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ABSTRACT 


The  potential  uses  for  autonomous  underwater  vehicles  (AUV's)  is  gaining 
recognition  by  organizations  world  wide.  As  such,  continuous  research  toward 
improving  existing  vehicles  by  seeking  new  designs  and  increasing  efficiency  is 
being  hmded  by  organizations  that  serve  to  benefit  from  the  new  technology.  Some 
examples  for  which  AUV's  may  be  used  include  mine  countermeasures,  submerged 
structural  repair  or  destmction,  search  and  rescue,  biological  study,  ocean  floor  and 
coastal  survey. 

The  goal  of  this  thesis  is  to  design  an  autopilot  that  will  use  a  combination 
of  both  vertical  thrusters,  both  horizontal  thrusters,  and  the  main  screws 
simultaneously  to  control  the  NFS  Phoenix  AUV's  posture  during  hovering 
conditions  and  short  transits.  The  control  design  is  implemented  and  simulated 
using  Common  LISP  object  oriented  programming  language.  The  results  of  this 
thesis  are  favorable.  Since  this  thesis  presents  only  a  basic  approach  to  an  autopilot 
design,  it  is  believed  by  the  author  that  with  further  improvements  to  the  design 
presented,  the  existing  hover  mode  autopilot  can  evenmally  be  upgraded  with  the 
resulting  autopilot  design.  This  upgrade  would  greatly  increase  the  autopilots 
efficiency. 
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I.  INTRODUCTION 


A.  NFS  PHOENIX  AUV 

The  Naval  Postgraduate  School  Phoenix  Autonomous  Underwater  Vehicle  (AUV) 
is  a  second  generation  autonomous  underwater  vehicle  developed  at  the  Naval  Postgraduate 
School.  Research  in  autonomous  underwater  vehicles  at  the  Naval  Postgraduate  School 
began  in  1987.  The  objective  of  this  thesis  is  to  design  and  simulate  a  hover  mode 
autopilot  that  will  use  a  combination  of  the  two  vertical  thrusters,  the  two  lateral  thrusters 
and  the  main  screws  to  control  the  Phoenix's  posture  during  hovering  and  short  transits 
when  the  low  speed  of  the  vehicle  does  not  allow  efficient  use  of  the  control  surfaces. 
Linear  control  theory  is  used  to  design  the  autopilot  based  upon  a  derived  mathematical 
model  that  represents  the  equations  of  motion.  The  design  is  implemented  and  simulated 
using  LISP  object  oriented  programming.  The  vehicle  dynamics  are  modeled  using  the 
Newton-Euler  three-dimensional  dynamic  equations.  It  is  these  equations  that  the  autopilot 
responds  to  in  order  to  determine  the  required  thruster  forces. 

B.  ORGANIZATION  OF  THESIS 

This  section  presents  the  organization  of  this  thesis.  Brief  descriptions  of  the 
chapter  contents  are  discussed. 

Chapter  II  introduces  several  different  concepts  being  investigated  by  various 
research  organizations.  It  also  presents  the  current  autopilot  techniques  implemented  in 
the  NPS  Phoenix  AUV. 

Chapter  III  defines  the  specific  problem  addressed  in  the  design  of  the  autopilot. 
Boundaries  surrounding  the  design  and  simulation  of  the  hover  mode  autopilot  are 
established  and  listed  as  initial  assumptions  and  limitations. 
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Chapter  IV  dcx^uments  the  procedure  followed  in  the  design  of  the  autopilot.  The 
equations  of  motion  are  derived  and  provide  the  mathematical  model  for  which  the  control 
equations  are  developed. 

Chapter  V  discusses  the  underlying  concepts  in  the  simulation  of  the  vehicle 
dynamics  as  well  as  the  techniques  for  a  pictorial  representation  of  the  vehicle’s  response 
the  autopilot  design. 

Chapter  VI  presents  the  simulation  results.  Several  observations  are  discussed. 
Chapter  VII  summarizes  the  thesis  with  recommendations  for  further  study. 


2 


n.  SURVEY  OF  PREVIOUS  AND  CONCURRENT  WORK 


A.  INTRODUCTION 

Autonomous  underwater  vehicles  (AUV's)  are  gaining  increased  attention  by 
research  and  development  institutions  world  wide.  The  goal  of  this  chapter  is  to  inform 
the  reader  of  ongoing  AUV  autopilot  research  projects  by  several  different  organizations. 
It  will  by  no  means  be  all  inclusive.  This  chapter  will  begin  with  brief  descriptions  of 
different  types  of  flight  control  autopilots;  however,  this  thesis  is  concerned  with  the 
design  of  an  autopilot  for  hovering,  meaning  control  by  use  of  thrusters  and  propellers 
only.  The  main  difference  between  a  flight  control  autopilot  and  a  hovering  autopilot  is 
that  a  flight  control  autopilot  can  utilize  control  surfaces  (fins  and  rudders)  in  conjunction 
with  its  forward  motion  propulsion  system  to  control  its  attitude,  whereas  a  hover  mode 
autopilot  cannot  due  to  the  lack  of  speed  generated  when  the  vehicle  is  trying  to  maintain 
a  specific  posture  or  short  distance  transits.  A  synopsis  of  the  current  autopilot 
implementation  for  the  NPS  Phoenix  AUV  will  follow.  Finally,  discussion  of  previously 
developed  software  used  for  the  simulation  of  the  hover  mode  autopilot  design  is  included. 

B.  BRIEF  DESCRIPTION  OF  VARIOUS  AUV  MODELS 

With  the  increased  interest  in  AUV's,  there  have  been  many  organizations  that  have 
designed  and  built  AUV's  for  various  missions  (Yuh,  1994).  The  criteria  necessary  for 
a  vehicle  to  be  considered  an  AUV  are  that  the  vehicle  be  unmanned,  untethered,  and 
submersible.  Hence  the  physical  design  has  taken  on  various  shapes,  and  since  mission 
is  not  part  of  the  criteria,  most  of  the  AUV's  designed  have  been  for  a  specific  mission. 
In  this  section  a  brief  description  of  some  of  the  current  AUV  research  will  be  presented. 
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1.  NDRE-AUV 

The  Norwegian  Defense  Research  Establishment  (NDRE)  designed,  built  and  tested 
a  flight  control  system  for  their  autonomous  underwater  vehicle  (NDRE-AUV)  which  was 
built  primarily  as  a  test  platform  for  propulsion  systems  using  seawater  batteries  (Jalving, 
1994).  The  autopilot  for  the  motion  control  was  divided  into  three  separate  controllers, 
one  each  for  speed,  steering,  and  diving  control.  NDRE  made  their  controller  robust  by 
applying  redundant  sensors  and  by  increasing  the  margins  of  stability  through  design.  In 
this  vehicle,  roll  dynamics  are  neglected,  and  thus  only  five  of  the  six  degrees  of  freedom 
are  controlled.  It  is  discussed  later  how  the  NFS  AUV  also  has  separate  controllers  for 
flight  control;  however.  Multivariable  Sliding  Mode  Control  concepts  were  used  to 
incorporate  robustness  into  the  controller  (Healey,  1993). 

2.  AQUA  EXPLORER  AEIOOO 

The  AQUA  EXPLORER  AEIOOO,  a  cable  tracking  AUV  built  in  Japan  by  KDD 
R&D  Laboratories,  uses  a  fuzzy  controller  in  their  implementation  of  the  autopilot  (Kato, 
1994).  The  fuzzy  control  concept  involves  the  representation  of  control  rules  in  a 
linguistic,  qualitative  manner.  In  the  case  of  the  AQUA  EXPLORER,  it  required 
transforming  each  sensor  input  into  five  fuzzy  variables  negative  big  (NB),  negative  small 
(NS),  zero  (Z),  positive  small  (PS)  and  positive  big  (PB).  These  five  variables  are  then 
associated  to  each  other  by  a  set  of  fuzzy  rules  defined  in  (Kato,  1994).  The  control 
outputs  are  then  obtained  and  applied  to  the  thrusters  and  fins.  All  six  degrees  of  freedom 
are  controlled.  The  applicability  of  fuzzy  control  for  a  cable  tracking  AUV  is  quite 
feasible,  since  for  its  specific  mission,  boundaries  for  the  AUV  to  operate  within  are 
required.  ARPA  also  has  developed  and  simulated  fuzzy  control  for  depth  and  pitch 
control  (DeBitetto,  1994). 
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3.  PTEROA 

A  Self-Organizing  Neural-Net-Controller  System  (SONCS)  is  an  adaptive  motion 
control  system  that  the  Institute  of  Industrial  Science,  University  of  Tokyo  has  developed 
for  use  in  their  AUV  called  PTEROA  (Fujii,  1990).  The  primary  mission  of  the  PTEROA 
is  to  take  pictures  of  the  sea  bed.  Although  a  Fuzzy  Controller,  as  in  the  AQUA 
EXPLORER  AEIOOO,  is  used  to  initialize  the  process  of  the  controller,  the  algorithm  used 
to  implement  the  fuzzy  logic  is  quite  different.  The  neural  network  concept  is  used  for  its 
advantages  of  parallel  processing  and  adaptability  (or  ability  to  learn).  Neural  networks 
also  have  been  proven  to  provide  fast,  accurate  solutions  to  nonlinear  problems  as  is  often 
found  in  control  designs.  However,  neural  networks  are  not  always  fast  in  convergence, 
and  a  PID  controller  can  be  designed  to  overcome  this  problem.  From  the  results  of 
testing,  it  is  believed  by  the  authors  that  highly  reliable  and  robust  control  systems  for 
AUV’s  are  possible  through  use  of  the  SONCS. 

4.  MUST 

Martin  Marietta  Aero  and  Naval  Systems  have  designed  a  flight  control  system 
based  on  Sliding  Mode  Control  concepts  and  implemented  it  on  their  Mobile  Undersea 
Systems  Test  (MUST)  unmanned  underwater  vehicle  (UUV)  (Dougherty,  1990).  The 
Sliding  Mode  Control  concept  was  used  in  the  design  of  both  the  transit  flight  control 
system  and  the  hover  flight  control  system.  As  in  the  NPS  Phoenix  AUV,  hover  is  defined 
as  the  operating  region  where  the  thrusters  and  main  screws  are  used  for  dynamic  control. 
As  part  of  the  test  for  robustness  of  the  hover  flight  control  system,  a  vertical  thruster 
failme  was  induced  during  a  maneuver.  The  system  response  was  favorable  in  that  after 
an  initial  transient,  the  MUST  UUV  became  stable  with  a  constant  error  in  pitch  and 
depth,  thus  validating  that  Sliding  Mode  Control  is  robust  in  the  face  of  control  system 
uncertainties. 
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5.  JASON  ROV 

The  JASON  ROV  was  developed  by  the  DEEP  SUBMERGENCE  Laboratory  of 
the  Woods  Hole  Oceanographic  Institute  (Yoerger,  1986).  Since  this  vehicle  is  an  ROV 
(remotely  operated  vehicle)  it  requires  an  operator  to  maneuver  it  by  controlling  the 
vehicle  with  a  joystick  and  monitoring  the  movement  through  a  video  camera  mounted  on 
the  vehicle.  A  supervisory  control  system  was  designed  for  implementation  using  closed 
loop  control  based  on  Sliding  Mode  Control.  This  type  of  control  simplifies  the  operators 
job  in  maneuvering  the  vehicle  in  that  the  operator  only  has  to  provide  continuous 
commands  for  the  desired  trajectory  while  the  system  provides  automatic  control  of  vehicle 
position.  While  this  is  not  exactly  an  autopilot  since  there  is  direct  operator  interaction, 
the  control  system  presented  can  be  utilized  in  designing  an  autopilot.  Among  the  modes 
simulated  for  this  vehicle  were  full  trajectory  control  in  body  coordinates  and  earth 
coordinates.  The  full  trajectory  control  in  body  coordinates  is  implemented  using  small 
angle  displacements  in  which  the  coordinate  system  is  constantly  changing,  that  is,  the 
"commands  are  defined  in  a  coordinate  frame  that  moves  with  the  vehicle"  (Yoerger, 
1986).  However,  full  trajectory  control  in  earth  coordinates  provides  a  coordinate  frame 
that  is  stationary  allowing  the  operator  to  maneuver  the  vehicle  more  smoothly  by  viewing 
a  map  display  vice  the  vehicle  video.  It  is  this  latter  type  of  control  on  which  this  thesis 
is  based  upon;  the  control  in  earth  coordinates  allows  for  large  angle  displacements  about 
the  fixed  earth  coordinate  system  as  compared  to  the  conventional  small  angle 
displacements  associated  with  the  body  coordinate  system. 

The  preceding  discussion  was  presented  to  introduce  the  reader  to  several  different 
control  methods  that  are  being  studied  and  used  by  various  organizations.  Robust  control 
is  an  important  issue  and  required  to  satisfy  the  uncertainty  in  the  vehicle  dynamics. 
Although  only  a  few  examples  were  given,  it  is  sufficient  to  say  that  the  types  of  control 
methods  introduced.  Sliding  Mode  Control,  Fuzzy  Control  and  Neural  Network  Control, 
are  all  techniques  that  are  being  studied  for  the  design  of  autopilots.  Although  it  appears 
that  no  single  method  is  better  suited,  the  classical  PID  controller  can  perform  well  in  both 
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a  flight  control  autopilot  as  well  as  a  hovering  autopilot  and  is  used  in  the  hover  mode 
autopilot  designed  in  this  thesis. 

C.  AUV  AUTOPILOTS  AT  NFS 

The  current  autopilot  design  for  the  NPS  Phoenix  AUV  is  based  on  Multivariable 
Sliding  Mode  Control  concepts  (Healey,  1993).  The  Sliding  Mode  Control  concept 
requires  that  sliding  surfaces  in  the  state  error  space  be  defined  such  that  a  sufficient 
relationship  for  the  control  element  forces  is  obtained.  This  assures  stability  of  the  state 
variable  errors  and  provides  acceptable  performance  with  closed  loop  conditions.  The 
autopilot  is  divided  into  three  separate  subsystem  autopilots  which  are:  the  Speed  Control 
Autopilot,  the  Steering  Autopilot,  and  the  Diving  Autopilot.  The  autopilot  described  in 
(Healey,  1993)  is  designed  for  transit  mode,  thus  constant  forward  motion  is  required  in 
order  to  maintain  control  of  the  AUV's  position.  More  recently,  hover  mode  control  in 
a  test  tank  environment  using  high  frequency  onboard  sonar  returns  without  the  use  of  a 
transponder  net  to  maintain  both  lateral  and  longitudinal  position  has  been  demonstrated 
(Torsiello,  1994).  Details  of  modelling,  sensor  calibration,  and  control  law  design 
appeared  in  (Torsiello,  1994).  The  position  control  demonstrated  longitudinal,  lateral,  and 
rotational  control  as  separate,  non-interacting  control  modes;  however,  the  combined 
action  of  longitudinal,  lateral,  and  rotational  control  is  what  needs  to  be  explored. 

D.  PREVIOUSLY  DEVELOPED  SOFTWARE 

All  of  the  code  used  for  the  implementation  and  simulation  of  the  hover  mode 
autopilot  design  for  this  thesis  is  written  in  LISP.  Some  MATLAB  code  is  used  for  ease 
of  plotting  the  results  of  the  simulation.  A  file  named  "robot-kinematics.cl"  is  used  in  its 
entirety  without  any  modifications.  This  file  contains  functions  that  were  developed  by 
Professor  McGhee  (McGhee,  1994)  to  allow  linear  algebra  manipulations  in  support  of 
kinematic  modeling.  It  also  incorporates  functions  that  perform  homogenous 
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transformations  which  are  required  for  three-dimensional  rotation  and  translation  of 
objects.  Another  previously  developed  file  (McGhee,  1994)  used  in  its  original  form  is 
"strobe-camera. cl".  "Strobe-camera.cl"  has  functions  that  create  a  window  for  the  object 
to  be  displayed.  It  uses  the  homogenous  transformation  matrix  of  the  object  to  move  the 
object  within  the  window,  hence  creating  the  illusion  of  three-dimensional  movement. 
Finally,  some  functions  developed  in  the  file  "rigid-body. cl"  are  reused  in  a  file  named 
"auv-rigid-body.cl".  These  include  a  function  to  create  a  rigid-body  class,  implement  the 
Newton-Euler  equations,  and  integrate  the  Newton-Euler  equations.  The  rest  of  the  LISP 
code  is  original. 

E.  SUMMARY 

As  can  be  ascertained  from  the  discussion  in  this  chapter,  research  and  development 
of  AUV's  is  a  relatively  new  field  of  interest  that  is  being  studied  by  organizations 
throughout  the  world.  Recognition  of  its  potential  uses  such  as  mine  countermeasures, 
hydrographic  and  ocean  floor  mapping,  submerged  structure  inspection,  repair  or 
destruction,  marine  life  studies,  hazardous  material  spill  assessments,  etc.,  will  ensure 
continued  and  improved  research  (Yuh,  1995).  NPS  has  taken  an  active  role  in  this 
research  arena  since  1987  with  emphasis  on  development  of  control  technology  for  AUV’s 
(Healey,  1993).  This  thesis  provides  the  basis  for  further  steps  in  this  direction  by 
designing  and  simulating  a  hover  mode  autopilot  and  investigating  the  use  of  LISP  as  a 
simulation  language  for  numerical  simulations  of  autopilots.  The  following  chapters  will 
discuss  in  detail  the  problem  addressed,  the  design  procedure,  and  evaluation  of  the  results 
obtained. 
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m.  DETAILED  PROBLEM  DESCRIPTION 


A.  INTRODUCTION 

The  objective  of  this  thesis  is  to  design  a  hover  mode  autopilot  for  the  NPS  Phoenix 
AUV  and  simulate  vehicle  response  using  Common  LISP  object  oriented  programming. 
The  purpose  of  this  chapter  is  to  provide  a  description  of  the  specific  problem  addressed 
in  the  design  of  the  hover  mode  autopilot.  This  chapter  begins  with  a  detailed  problem 
statement  followed  by  a  discussion  of  the  assumptions  and  limitations  for  which  the  hover 
mode  autopilot  controller  is  designed. 

B.  DETAILED  PROBLEM  STATEMENT 

The  goal  of  this  thesis  is  to  design  an  autopilot  for  controlling  azimuth,  elevation, 
forward  displacement,  lateral  displacement,  and  depth  of  the  AUV  using  only  the  two 
vertical  thrusters,  the  two  lateral  thrusters,  and  the  main  screws.  Note  that  there  is  no 
attempt  to  actively  control  roll.  This  is  allowed  since  the  AUV  was  designed  with  a  high 
degree  of  roll  stability  when  submerged  by  using  the  natural  roll  righting  moment  as  the 
correcting  force  and  the  hydrodynamic  effects  of  the  medium  as  the  damping  factor. 
Thus,  there  are  six  degrees  of  freedom,  but  only  five  are  actively  controlled.  The  actual 
posture  of  the  AUV  is  to  be  determined  by  the  navigation  system  where  it  is  assumed  that 
the  position  x,  y,  z,  and  the  attitude  as  measured  by  the  global  Euler  angles,  with  the 
exception  of  roll,  are  available  as  sensed  data.  For  the  purpose  of  the  design  and 
simulation,  the  Newton-Euler  dynamic  equations  are  used  to  determine  the  translational 
and  rotational  velocities  and  positions  (posture).  The  autopilot  acts  to  compare  the  actual 
posture  with  the  desired  posture.  Any  differences  between  the  actual  posture  and  desired 
posture  are  used  to  determine  the  thruster  forces  determined  by  Global  Frame  sensed  data 
applied  in  the  local  body  fixed  frame  to  position  the  AUV  until  the  actual  posture  and 
desired  posture  are  equal.  The  following  section  describes  the  limitations  of  the  design 
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which  are  essential  to  setting  bounds  for  this  thesis.  This  work  is  contrasted  to  other  work 
in  multidimensional  positioning  (e.g.  Offshore  drilling  vessels)  where  the  departure  of 
angular  positions  from  a  nominal  position  is  small. 

C.  DESIGN  PARAMETERS 

This  section  discusses  the  design  environment  for  which  the  hover  mode  autopilot 
controller  is  designed.  It  emphasizes  the  definition  of  hover  mode  and  identifies  the 
assumptions  and  restrictions  utilized  for  the  initial  autopilot  design. 

As  previously  stated,  hover  mode  control  is  control  by  use  of  thrusters  and  main 
screws  only.  To  elaborate,  hover  mode  includes  those  periods  of  dynamic  control  when 
the  AUV  is  trying  to  maintain  a  specific  posture  relative  to  a  fixed  earth  coordinate 
system.  It  also  includes  short  transits  where  the  AUV's  speed  will  not  be  sufficient  for  the 
fms  to  be  used  as  control  surfaces  such  as  in  transit  mode. 

For  the  initial  design  of  the  hover  mode  autopilot  controller,  severed  assumptions 
and  restrictions  are  imposed.  This  is  done  to  set  the  basic  foundation  for  the  current 
design  and  so  that  future  work  will  have  a  foundation  to  build  upon.  The  assumptions 
used  are  as  follows: 

-  Angular  rate  sensors  are  perfect. 

-  Accelerometers  are  perfect. 

-  Thrusters  are  ideal  with  the  exception  of  saturation,  that  is,  when  thrusters  reach 
their  maximum  available  thurst  force. 

-  Propulsion  from  main  screws  is  ideal. 

-  Tracking  of  continuous  trajectories  is  perfect. 

-  There  are  only  small  deviations  from  the  desired  posture,  that  is,  the  attitude  and 
translational  position. 

The  restrictions  imposed  are  as  follows: 

-  Hydrodynamic  drag  is  neglected  (due  to  low  vehicle  speed). 
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-  Hydrodynamic  added  mass  is  limited  to  diagonal  terms  (actual  added  mass  effects 
are  not  precisely  known  for  the  Phoenix  AUV). 

-  There  are  no  disturbances. 

-  The  main  screws  operate  at  the  same  revolutions  per  minute  and  are  treated  as 
a  single  thruster. 

-  The  vehicle  has  positive  buoyancy. 

The  actual  design  of  the  hover  mode  autopilot  controller  is  simplified  with  the  design 
parameters  defined  above.  The  following  chapter  defines  the  model  used  and  presents  the 
development  of  the  hovering  autopilot. 

D.  SUMMARY 

The  preceding  paragraphs  present  the  problems  addressed  in  the  design  and 
simulation  of  the  NPS  Phoenix  AUV  hover  mode  autopilot.  The  design  parameters  for 
which  the  autopilot  is  developed  are  presented.  The  defined  design  parameters  consists 
of  assumptions  and  restrictions  which  provide  a  means  to  bound  the  design  efforts.  This 
chapter  includes  a  description  of  the  problem  from  which  the  design  efforts  were  focused. 
The  next  two  chapters  will  present,  in  detail,  the  system  design  and  simulation  design, 
respectively. 
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IV.  SYSTEM  DESIGN 


A.  INTRODUCTION 

This  chapter  will  present  in  detail  the  design  procedure  followed  in  the  development 
of  the  hover  mode  autopilot  controller  for  the  NFS  Phoenix  AUV,  In  designing  the  hover 
mode  autopilot  for  the  NPS  Phoenix  AUV,  the  vehicle  dynamics  are  linearized  in  the  earth 
coordinate  system  and  then  transformed  to  the  body  coordinate  system.  This  choice  was 
made  because  observation  or  interaction  with  an  underwater  object  that  uses  a  navigation 
system  involves  maneuvering  in  earth  coordinates,  not  body  coordinates. 

In  order  to  design  the  hover  mode  autopilot,  the  problem  is  divided  into  three  parts. 
First,  the  equations  of  motion  for  each  plane  of  a  three-dimensional  earth  fixed  coordinate 
system  are  determined.  Second,  linear  control  theory  concepts  are  used  to  chose  the 
control  law  equations  and  also  to  determine  the  gains  required  for  feedback  control. 
Finally,  the  necessary  thruster  forces  in  response  to  the  autopilot  are  calculated  using  linear 
transformations. 

The  next  chapter  will  discuss  the  simulation  of  the  NPS  Phoenix  AUV  in  response 
to  the  autopilot.  The  following  sections  describe  the  design  procedure  and  present  the 
analytical  steps. 

B.  EQUATIONS  OF  MOTION 

The  first  section  of  this  chapter  discusses  the  equations  of  motion.  Since  the 
control  design  is  linearized,  we  begin  by  dividing  the  three-dimensional  space  into  three 
separate  planes  with  the  AUV's  body  coordinate  system  initially  aligned  with  the  fixed 
earth  coordinate  system.  The  planar  views  are  arranged  such  that  for  the  top  view  (Figure 
1),  the  positive  z  axis  is  directed  into  the  plane  of  the  paper;  the  positive  y  axis  comes  out 
of  the  plane  of  the  paper  for  the  side  view  (Figure  2);  and  finally,  for  the  stern  view 
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(Figure  3),  the  positive  x  axis  goes  into  the  plane  of  the  paper.  Newton’s  second  law, 
force  equals  mass  times  acceleration,  was  used  to  obtain  the  equations  of  motion.  Note 
that  the  forces  due  to  gravity  and  buoyancy  are  taken  into  account.  Figure  1  through 
Figure  3  are  referred  to  when  deriving  the  equations  of  motion.  In  all  three  figures  the 
origin  also  represents  the  center  of  gravity.  For  the  NFS  Phoenix  AUV,  the  center  of 
buoyancy  (Cb)  is  the  geometric  center  of  the  vehicle  when  submerged  and  is  stationary. 
This  is  possible  regardless  of  the  posture  because  the  hull  cavity  aspect  does  not  change 
for  a  submerged  vessel  as  opposed  to  a  surface  vessel  whose  partially  submerged  hull 
cavity  physically  changes  with  pitch  and  roll  and  because  the  NFS  Phoenix  AUV  does  not 
use  variable  ballast  as  is  typical  of  manned  submarines. 

The  equations  of  motion  for  the  top  view  plane  (Figure  1)  can  be  obtained  by  using 
Newton’s  second  law.  Since  the  two  axes  that  form  the  plane  of  Figure  1  are  the  Xg  and 
Ye  axis,  the  motions  and  forces  concerned  are  described  by  these  two  coordinates.  By 
studying  Figure  1,  we  determine  that  the  force  in  the  Xe  direction  is  equal  to  the  mass 
times  the  acceleration  in  the  Xe  direction.  We  then  rearrange  the  terms  to  solve  for 
acceleration  which  yields  the  following  equation: 

F 

^  / 1  \ 


where  denotes  the  acceleration  along  the  Xe  axis,  m  is  the  mass  of  the  vehicle 
and  is  the  force  applied  to  the  vehicle  along  the  Xe  axis.  A  similar  argument  for  the 
force  in  the  yn  direction  yields: 


m 


where  and  denotes  the  acceleration  and  force  applied  along  the  yE  axis 
respectively. 
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Top  View  of  Yaw  Motion  Plane 
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Figure  2 

Side  View  of  Pitch  Motion  Plane 


T2 
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Figure  3 

Stem  View  of  Roll  Motion  Plane 
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There  is  one  more  force  to  contend  with  in  this  plane,  and  that  is  the  torque  about  the  Zg 
axis.  As  stated  previously,  the  Zg  axis  is  perpendicular  to  the  Xe-Ye  coordinate  plane  with 
positive  Ze  going  into  the  plane  of  the  paper.  Here  again  Newton's  second  law  can  be 
applied  to  obtain  a  motion  equation.  By  associating  the  force  to  torque,  the  mass  to  the 
moment  of  inertia  and  the  angular  acceleration  to  acceleration,  we  obtain  Equation  (3) 
from  Figure  1: 


where  f  denotes  the  angular  acceleration,  is  the  torque  about  the  Ze  axis  and  is 
the  inertia  about  the  z  axis.  The  total  torque  about  the  Ze  axis  is  determined  by  the  sum  of 
thruster  force  times  the  distance  of  the  respective  thruster  from  the  center  of  gravity.  The 
forces  and  distances  depicted  in  Figure  1  are  used  to  determine  Equation  (4): 

Ty  .  -T4-d4  *  TS  dS  (4) 

where  T4d4  and  T5d5  are  the  aft  and  forward  lateral  thrusters  multiplied  by  their 
respective  distances  from  the  center  of  gravity. 

For  the  side  view  (Figure  2),  the  two  axes  that  form  the  coordinate  plane  of 
concern  are  the  Xe  and  Zb  axes.  Since  the  Xg  of  the  side  view  coincides  with  that  of  the  top 
view,  the  equation  of  motion  is  the  same;  thus.  Equation  (1)  is  used  for  both  the  Xg-Zg  and 
the  Xg-yg  planes.  When  dealing  with  the  Zg  axis,  there  are  two  additional  forces  to 
include,  buoyancy  and  weight,  which  are  always  applied  at  the  center  of  buoyancy  and  the 
center  of  gravity,  respectively.  The  vehicle  is  operated  with  positive  buoyancy  so  that  in 
the  event  of  a  system  failure  that  causes  loss  of  power,  the  vehicle  will  float  to  the  surface 
for  recovery.  The  equation  of  motion  in  the  Zg  direction  derived  from  Figure  2  is  : 
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(5) 


.  Fw  Fh 

^  m  mm 

As  in  the  Xe-y-E  coordinate  plane,  there  is  also  a  force  due  to  a  torque  involved;  however, 
in  this  case  the  torque  is  about  the  yg  axis.  By  observing  Figure  2  and  following  the  same 
procedure  as  before,  we  get  Equation  (6): 


where  e  denotes  the  angular  acceleration  about  the  yg  axis,  r,  represents  the  torque 
about  the  yg  axis,  and  lyy  is  the  inertia.  In  order  to  determine  the  total  torque  from  Figure 
2,  a  more  involved  analysis  is  required  due  to  the  righting  moment  effect  caused  by  the 
buoyancy  (Fb)  and  gravitational  forces  (Fw).  By  applying  vector  analysis  rules,  we  can 
obtain  the  following  torque  due  to  the  righting  moment: 


(7) 


The  total  torque  about  the  yg  axis  is  then  determined  by  summing  all  the  torques  about  the 
yg  axis,  thus  yielding  Equation  (8). 


r,  -  TI‘  dl  -T2‘  d2  -  T3-  d3  -  (8) 

The  axes  for  the  stem  view  (Figure  3)  are  yg  and  Zg.  The  equations  of  motion  for 
these  two  axes  have  already  been  determined  from  the  previous  two  coordinate  planes 
discussed.  The  remaining  force  in  the  yg-Zg  coordinate  plane  is  the  torque  about  the  Xg 
axis: 
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where  ^  denotes  the  angular  acceleration  about  the  Xe  axis,  represents  the  torque 
about  the  Xe  axis,  and  I^x  is  the  inertia.  In  this  case  there  is  no  active  means  to  control 
roll;  therefore,  the  hydrodynamic  damping  effect  is  used  to  counter  the  torque  induced  by 
the  lateral  thrusters  and  the  righting  moment  due  to  the  rolling  effect.  Thus,  the  equation 
for  torque  about  the  Xe  axis  is 

-  T4-  03  *  T5^  03  -  Fb'  dS'  sin^)  -  p  (10) 

All  the  equations  of  motion  for  the  three  planes  have  been  derived.  In  order  to  use 
these  equations,  some  assumptions  and  substitutions  will  be  incorporated  into  them.  First, 
since  the  vehicle  will  be  operating  completely  submerged  there  is  an  added  mass  effect  due 
to  the  water  that  will  be  moving  with  the  AUV.  These  terms  are  identified  as  surge,  sway, 
heave,  roll,  pitch  and  yaw  hydrodynamic  coefficients  and  have  been  approximated  in 
(Warner,  1991).  However,  only  the  diagonal  terms  (  z^,  )  will  be  used 

as  mentioned  in  Chapter  III  under  the  limitations  imposed.  Hence,  the  mass  and  inertias 
are  divided  into  three  components  each  where  the  mass  equations  become 


(10a) 

(10b) 

(10c) 


and  the  moments  of  inertia  equations  become 


I j 

XX  IOC  p 


(lOd) 

(lOe) 


The  average  of  niy,  and  denoted  as  m  ,  is  substituted  into  Equations  (1),  (2)  and 
(5)  to  yield  Equations  (11),  (12)  and  (13),  respectively.  For  the  moments  of  inertia  a 
similar  approach  is  taken  except  that  since  Ixx  is  small  compared  to  lyy  and  it  is 
neglected.  The  average  of  lyy  and  is  used  and  denoted  as  I  which  is  substituted  into 
Equations  (3),  (6),  and  (9). 

The  next  consideration  is  the  torques  introduced  by  the  thruster  commands.  Note 
that  for  r,  there  are  no  nonlinear  terms  induced  by  the  horizontal  thrusters  T4  and  T5; 
hence,  the  resulting  torque  is  the  torque  commanded  and  is  denoted  as  r^cOTi-  r^eom  is 
substituted  into  Equations  (3)  and  (4)  to  yield  Equation  (14).  There  is,  however,  a 
nonlinear  term  involved  for  r#.  The  nonlinear  term  here  is  a  result  of  the  righting  moment 
due  to  the  buoyancy  force,  the  pitch  angle,  and  the  distance  between  the  center  of 
buoyancy  and  the  center  of  gravity.  Therefore,  the  torque  commanded,  Tecom,  is  only  part 
of  the  total  torque.  The  following  section  discusses  how  this  nonlinearity  is  dealt  with. 
For  now  let  the  conunanded  torque  be  defined  as 

=  Tldl-T2d2-T3d3  (lOg) 

and  substitute  it  into  Equations  (6)  and  (8)  to  obtain  Equation  (15).  Since  roll  is  not 
actively  controlled,  no  attempt  was  made  to  determine  a  command  torque  about  the  x  axis. 
A  summary  of  the  motion  equations  with  the  torque  equations  substituted  into  their 
respective  motion  equations  is  provided  in  Table  1.  Note  that  the  equations  have  been 
solved  for  the  forces  and  torques  and  no  attempt  has  been  made  to  derive  a  torque 
commanded  equation  for  the  moment  about  the  x  axis.  The  following  section  discusses 
the  control  theory  involved  for  determining  the  control  law  equations. 
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^Vccm-  -T4d4*T5d5  (14) 

(12) 

e/.  Tldl  -T2d2  -T3d3  -Fhdisin  6  (15) 

T4d3  *T5d3  -FbdSsin  (16) 

Table  1 

Equations  of  Motion 


C.  CONTROL  THEORY 

Based  on  the  design  environment  defined  in  the  previous  chapter  and  the  model 
defined  in  the  previous  section,  linear  control  theory  is  selected  for  the  development  of 
the  hover  mode  autopilot  controller.  The  following  paragraphs  address  the  linear  control 
theory  concepts  used  in  the  design  of  the  autopilot. 

The  control  law  equations  were  selected  by  using  a  control  law  of  form 

f(t)  =  -  kiXi(t)  -  k2X2(t)  +  b(t)  (17) 

where  f(t)  is  the  resulting  force  in  earth  coordinates,  ki  and  k2  are  the  gains,  Xi(t)  and  X2(t) 
are  position  and  velocity  respectively,  and  b(t)  is  a  biasing  term.  The  biasing  term  is  used 
to  cancel  out  any  nonlinear  terms  resulting  from  the  righting  moments  due  to  interaction 
of  buoyancy  and  gravity.  Cancelling  out  the  nonlinear  terms  with  the  biasing  term 
linearizes  the  system. 

In  order  to  determine  the  gains  to  be  used  for  the  design,  the  damping  ratio  (C)  and 
the  natural  frequency  (wn)  must  be  known.  The  system  is  designed  to  be  slightly 
underdamped,  hence  0.8  was  selected  for  the  damping  ratio.  A  settling  time  of  125 
seconds  was  chosen  for  each  plane  based  on  an  assumption  that  it  was  a  reasonable  amount 
of  time  for  the  vehicle  to  move  fifteen  feet  forward,  ten  feet  laterally  and  five  feet  in 
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depth.  The  settling  time  equation  used  from  linear  control  theory  is 

ts  =  4/Cco„  (18) 

This  equation  is  based  on  a  tolerance  of  a  two  percent  range  of  the  desired  final  value 
(Thompson,  1989).  With  Equation  (18),  given  the  settling  time  and  damping  ratio,  the 
natural  frequency  is  calculated.  The  initial  conditions  are  set  to  zero  and  the  control  law 
equations  are  substituted  into  the  equations  of  motion  to  obtain  the  characteristic  equations 
which  are  of  the  form 

+  co„  =  0  (19) 


The  required  thruster  forces  are  then  obtained  by  linear  transformation  of  the  forces 
in  earth  coordinates.  The  forces  in  body  coordinates  are  calculated  by  extracting  the 
direction  cosine  transformation  matrix  from  the  homogeneous  transformation  matrix, 
discussed  in  the  next  chapter,  and  pre-multiplying  the  result  with  the  forces  in  earth 
coordinates  which  will  be  determined  from  the  control  law  equations.  The  inverse  of  the 
thruster  coefficient  matrix  is  then  pre-multiplied  with  the  forces  in  body  coordinates  to 
obtain  the  necessary  thruster  forces  in  response  to  the  hover  mode  autopilot.  A  more 
detailed  analysis  of  the  preceding  discussion  is  presented  in  the  following  sections. 

D.  CONTROL  EQUATIONS 

In  order  to  simplify  the  design  of  the  hover  mode  autopilot,  the  system  is 
linearized.  This  is  accomplished  by  adding  or  subtracting  biasing  terms  where 
nonlinearities  exist.  A  linear  feedback  control  law  of  the  form  as  Equation  (17)  is  chosen 
for  each  of  the  forces  and  torques  presented  in  Table  1  except  for  Equation  (16)  which  is 
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not  actively  controlled.  Additionally,  note  that  the  torque  about  the  Zg  axis  has  no 
nonlinear  terms  which  allows  it  to  be  controlled  by  using  the  lateral  thrusters  T4  and  T5. 
For  the  torque  about  the  axis  there  does  exist  a  nonlinear  term  which  is  the  result  of  the 
righting  moment  effect  due  to  buoyancy  and  gravity.  As  discussed  earlier,  this  is  resolved 
by  adding  a  biasing  term  to  the  control.  Also  note  that  constants  are  included  for  the 
control  equations  in  the  Ze  direction  to  compensate  for  the  effects  of  buoyancy  and  gravity. 
The  resulting  equations  are  as  follows: 


(20) 

h  (21) 

^  (22) 

^  (23) 

^aow  •  6  *  Fh'  d3‘  (24) 


Now  that  the  control  equations  have  been  determined,  the  necessary  gains  can  be 
calculated.  Substituting  Equations  (11)  -  (13)  into  Equations  (20)  -  (24)  and  solving  for 
zero,  one  obtains  the  following: 


*E  *  —Xe  "  -  0 

m  nt 

(25) 

Fe  *  -rXs  »  -rO'rJ'o)  •  0 
m  m 

(26) 

‘e  *  —^E  *  ® 

m  m 

(27) 

0  ^  -^0  ^  .  0 

(28) 

^  ,  km 

(29) 
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which  are  of  the  form  as  Equation  (19)  presented  earlier.  If  one  sets  the  initial  conditions 
to  zero  and  associates  like  terms  to  that  of  Equation  (19),  the  calculation  of  the  various 
gains  is  made  possible.  Since  the  settling  time  was  chosen  to  be  the  same  for  each  control 
equation,  all  like  terms  for  the  gains  were  set  equal  to  each  other.  Given  a  settling  time 
of  125  seconds  and  a  damping  ratio  of  0.8,  Equation  (18)  is  solved  for  the  natural 
frequency  which  results  as  .04  radians  per  second.  The  gains  are  determined  by  using  the 
following  formulas: 


(29a) 

(29b) 

(29c) 

(29d) 

for  the  forces  and  torques  respectively.  All  that  remains  for  the  design  of  the  hover  mode 
autopilot  is  to  use  the  previously  derived  control  equations  and  gains  to  determine  the 
required  thruster  forces,  which  is  discussed  in  the  next  section. 

E.  THRUSTER  EQUATIONS 

The  thruster  forces  in  response  to  the  derived  control  equations  are  determined  in 
this  section.  This  is  done  by  studying  the  thruster  vectors  depicted  in  Figures  1-3. 

The  forces  acting  on  the  vehicle  in  body  coordinates  are  observed  to  be  those 
resulting  from  the  thrusters;  hence,  the  resulting  body  forces  are 


Fx  =  T3 
Fy  =  T4  -I-  T5 
F,  =  T1  +  T2 
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(30) 

(31) 

(32) 


or  more  conveniently, 


F 


T3 

T4*TS 

T1*T2 


(33) 


In  order  to  determine  the  forces  in  body  coordinates,  the  forces  in  earth  coordinates  must 
be  transformed  into  body  forces.  The  convention  most  commonly  used  to  accomplish  this 
is  by  means  of  the  direction  cosine  transformation  matrix  discussed  in  (Cooke,  1992) 
which  defines  the  direction  cosine  transformation  matrix  as 


cosificosO  cosi|r5iii6sin4>-sini|icos4»  cos!|rsinOc0s4»*smi|;sin4» 

''ll  ''12  '’13 

R  . 

sini|/cos6  cosi|/cos4*^smi|rsin6siii4>  'Cosi|rsiii4>+sini|;sm6cos4» 

- 

rji  Tjj  »'23 

-sinO  cos0sdn4>  cosdcosi^ 

'’31  '•32  '•33 

The  transformation  from  body  to  earth  coordinates  is  given  by 


Fe  =  RF 


(34a) 


Thus,  using  matrix  algebra  yields  a  transformation  from  earth  to  body  coordinates  as 

F  =  R-^  Fe  (34b) 

Note  that  the  direction  cosine  matrix  has  a  unique  characteristic  in  that  its  inverse  is  equal 
to  its  transpose,  thus  R’^  =  R'*'  and  F  =  R'^  Fg  can  be  rewritten  as  F  =  R’’  Fe.  By 
substituting  Equations  (30)  -  (32)  for  F  and  Equations  (20)  -  (22)  for  Fg,  we  obtain  a  set 
of  three  equations  and  five  unknowns,  namely  the  thruster  forces  T1  through  T5.  The  two 
torque  equations  must  be  used  obtain  a  set  of  five  equations  and  five  unknowns  to  be  able 
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to  solve  for  the  thruster  forces.  By  combining  Equations  (14)  and  (15)  with  Equations  (23) 
and  (24) ,  respectively,  we  get 

-T4^  d4  .TS  dS  .  (35) 

Tl-  dl  -  T2-  d2  -  T3‘  d3  .  (36) 

If  we  group  Equations  (30)  -  (32)  and  Equations  (35)  -  (36)  into  a  matrix  form  and  solve 
for  the  thruster  vector,  the  following  result  is  obtained 


Tl 

0 

0 

1 

0 

0 

-1 

■ 

T2 

0 

0 

0 

1 

1 

T3 

m 

1 

1 

0 

0 

0 

T4 

0 

0 

0 

-d4 

dS 

r 

T5 

,d2 

d3 

0 

0 

(37) 


To  determine  the  Euler  angles  and  Euler  angle  rates  needed  in  the  torque  equations,  the 
rotational  velocities  of  the  vehicle,  measured  about  the  vehicles  body  coordinates,  are 
transformed  to  Euler  angle  rates  using  the  non-orthogonal  transformation  matrix  defined 
in  (Frank,  1969).  These  rates  are  then  integrated  to  obtain  the  Euler  angles.  For  the 
simulation  of  the  hover  mode  autopilot,  the  Newton-Euler  equations  are  used  to  provide 
the  body  translational  and  rotational  accelerations.  By  integrating  these  accelerations,  we 
get  the  body  translational  and  rotational  velocities.  This  process  will  be  discussed  in 
further  detail  in  the  next  chapter. 

The  horizontal  and  vertical  thrusters  have  a  maximum  thrust  force  of  approximately 
two  pounds.  The  combined  thrust  force  of  the  main  screws  is  approximately  ten  pounds. 
These  limits  have  been  implemented  into  die  simulation  model.  Thus,  if  a  desired  posture 
requires  more  force  than  is  available,  the  thrusters  will  saturate. 
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F.  SUMMARY 

This  chapter  presents  a  detailed  analysis  of  the  procedtu’e  followed  in  the  design  of 
the  hover  mode  autopilot  for  the  NFS  Phoenix  AUV.  A  simplified  mathematical  model 
is  the  basis  for  the  initial  autopilot  design  from  which  a  more  complex  model  may  be 
derived  in  future  work.  Control  theory  provides  the  essential  tools  for  which  the  hover 
mode  autopilot  controller  has  been  designed.  Although  the  actual  system  is  nonlinear,  the 
use  of  biasing  terms  allows  the  system  to  be  linearized  which  permits  the  use  of  linear 
control  theory  concepts  in  the  design  of  the  autopilot,  thus  reducing  the  complexity  of  the 
design.  The  equations  of  motion  were  determined,  followed  by  a  brief  section  on  control 
theory.  Next  the  derivation  of  the  control  equations  were  determined,  concluding  with  the 
development  of  the  thruster  equations.  The  results  obtained  fi-om  the  testing  and 
simulation  satisfy  the  requirements  and  are  discussed  in  Chapter  VI.  Chapter  V  presents 
the  simulator  design. 
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V.  SIMULATOR  DESIGN 


A.  INTRODUCTION 

The  simulation  of  the  hover  mode  autopilot  for  this  thesis  takes  on  two  phases, 
numerical  simulation  and  visual  simulation.  The  numerical  simulation  phase  produces  the 
values  as  calculated  from  mathematical  models  which  are  plotted  versus  time  such  that  they 
may  be  analyzed.  Visual  simulation  takes  the  values  produced  in  the  numerical  phase  and 
applies  them  to  a  visual  simulator  which  presents  a  pictorial  observation  of  how  the  vehicle 
is  responding  to  the  autopilot.  These  two  phases  are  performed  simultaneously.  The 
following  sections  will  present  the  procedure  for  simulator  design.  It  begins  with  the 
dynamic  equations,  followed  by  the  various  transformations,  and  the  method  used  to 
display  the  vehicle  movement,  concluding  with  the  force  and  moment  calculations. 

B.  DYNAMIC  EQUATIONS 

In  order  to  properly  simulate  any  system,  a  mathematical  model  must  be  developed 
which  closely  represents  the  system.  The  previous  chapter  derived  the  various 
mathematical  models  which  represent  the  type  of  control  to  be  used.  Here  the 
mathematical  model  for  the  vehicle  dynamics  will  be  illustrated. 

Before  continuing,  it  is  imperative  to  define  the  meaning  of  the  terms  that  will  be 
used.  Table  2  accomplishes  this  task  in  a  concise  manner.  The  terms  defined  in  Table  2 
and  Figures  4  -  6  are  used  to  derive  the  dynamic  equations.  Since  the  translational  and 
rotational  accelerations  in  the  body  reference  frame  are  the  most  convenient  to  measure, 
it  makes  sense  to  solve  for  the  accelerations.  Figure  4  depicts  a  body  coordinate  system 
with  the  various  velocities  necessary  to  solve  for  the  translational  acceleration  in  the  x 
direction  without  the  effect  of  gravity. 
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Figure  4 

longitudinal  Velocity  Growth  Rate  Derivation 
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Figure  5 

Lateral  Velocity  Growth  Rate  Derivation 
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Figure  6 


Vertical  Velocity  Growth  Rate  Derivation 
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Symbol 

Definition 

Units 

mm 

longitudinal,  lateral,  and  vertical  body  coordinate  axes  respectively 

ft 

u,  v,w 

X,  y,  z  translational  velocities 

ft/sec 

X,  y,  z  translational  velocity  growth  rates 

ft/sec^ 

♦,e,? 

roll,  elevation,  and  azimuth  in  earth  coordinates 

rad 

■m 

rotational  velocities  about  the  x,  y,  and  z  axes  respectively 

rad/sec 

p4/ 

rotational  velocity  growth  rates  about  the  x,  y,  and  z  axes  respectively 

rad/sec^ 

FxTy,  F, 

forces  acting  on  the  body  in  the  x,  y,  and  z  direction 

lbs 

K,  M,N 

torques  acting  on  the  body  about  the  x,  y  and  z  axes  respectively^ 

ft  lbs 

Ixx>  ^yy> 

moments  of  inertia  of  the  body  about  the  x,  y,  and  z  axes  respectively 

slugs  f^^ 

Table  2 

Conventional  Variable  Definitions 


The  total  translational  acceleration  in  the  x  direction  is  the  sum  of  all  the  x 
component  acceleration  vectors.  For  this  case,  there  are  three  such  components.  The  first 
term  is  due  to  the  effects  of  the  translational  velocity  in  the  y  direction  and  the  rotational 
velocity  about  the  z  axis.  Note  that  multiplying  a  translational  velocity  by  a  rotational 
velocity  will  yield  the  tangential  component  of  acceleration  which  is  mutually 
perpendicular  to  both  the  translational  velocity  vector  and  the  rotational  velocity. 
Similarly,  the  second  term  is  due  to  the  effects  of  the  translational  velocity  in  the  z 
direction  and  the  rotational  velocity  about  the  y  axis.  The  third  acceleration  component 
in  the  X  direction  is  due  to  the  force  being  applied  in  the  x  direction. 

There  is  actually  one  more  acceleration  vector  that  affects  the  body  and  that  is  the 
acceleration  due  to  gravity.  Since  gravity  is  a  constant  that  is  always  present,  it  will 


^  Although  tbe  equations  for  these  values  are  the  same  for  aircraft  and  ships,  the  convention  for  notation  is 
different.  When  dealing  with  aircraft,  the  torque  about  the  x  axis  is  denoted  by  L;  but  when  dealing  with  sh^s,  L  is 
reserved  for  length  and  so  K  is  used  instead. 
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always  affect  the  body.  Gravity  is  an  acceleration  in  earth  coordinates  and  must  be 
transformed  into  body  coordinates.  Let  the  vector  A  be  defined  as  [0  0  g]^,  the  constant 
accelerations  in  earth  coordinates.  The  acceleration  vectors  in  body  coordinates  are 
produced  by  R'^A  which  yields  the  following: 


Ux  =  -g  sin0 

(38) 

ay  =  g  COS0  sin<^ 

(39) 

a^  =  g  COS0  cos<^ 

(40) 

where  ax,  Uy,  and  a^  are  the  accelerations  due  to  gravity  in  the  x,  y,  and  z  directions 
respectively.  Thus,  the  following  equation  is  obtained  from  studying  Figure  4  and 
includes  Equation  (38)  of  the  previous  analysis  on  the  effects  of  gravity. 

F 

ti  -  vr  -  tvq  *  — ^  -  g  sinO  (41) 

«* 

Studying  Figure  5,  we  see  that  the  acceleration  in  the  y  direction  is  the  sum  of  the 
translational  velocity  (p)  times  the  rotational  velocity  (w),  the  translational  velocity  (u) 
times  the  rotational  velocity  (r),  and  the  acceleration  due  to  force  in  the  y  direction.  The 
acceleration  in  the  y  direction  due  to  gravity.  Equation  (39),  is  added  and  results  in 
Equation  (42).  The  same  procedure  is  followed,  this  time  using  Figure  6  and  including 
Equation  (40)  which  yields  Equation  (43). 

F 

V  -  wp  -  ur  *  —  *  g  cosO  sm4»  (42) 

F 

w  »  uq  -  yp  ^  ^  ♦  g  COS0  cos4>  (43) 
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The  angular  accelerations  are  obtained  by  analyzing  the  torques  involved.  First 
begin  with  the  angular  acceleration  about  the  x  axis.  The  torque  about  the  x  axis  is  equal 
to  the  rotational  velocity  growth  rate  about  the  x  axis  (  /»  )  times  the  moment  of  inertia 
(Ixx),  which  is  equal  to  the  sum  of  its  individual  components.  One  of  the  components  (K) 
is  merely  the  resultant  torque  of  the  implied  forces.  The  next  component  is  determined  by 
taking  the  angular  momentum  about  the  y  axis,  that  is  the  product  of  the  moment  of  inertia 
about  the  y  axis  (lyy)  and  the  rotational  velocity  about  the  y  axis  (q),  and  multiplying  it 
with  the  rotational  velocity  about  the  z  axis  (r).  Finally,  the  third  component,  is  obtained 
by  taking  the  angular  momentum  about  the  z  axis,  times  r,  and  multiplying  it  with  the 
rotational  velocity  about  the  y  axis  (-q).  Since  the  rotational  velocity  growth  rate  p  is  the 
term  desired,  both  sides  of  the  equation  are  divided  by  Ixx,  ultimately  resulting  in  Equation 
(44).  A  similar  analysis  in  determining  4  and  r  yields  Equations  (45)  and  (46). 


.  1  (V^rr)  9r 

P - - 


(44) 


[  UM  rp  .M] 

[  (  W  Pq  *  If  ] 


(45) 

(46) 


Equations  (41)  to  (46)  constitute  the  Newton-Euler  equations  used  as  the  mathematical 
model  to  represent  the  vehicle  dynamics.  These  equations  are  implemented  in  a  function 
named  "update-velocity-growth-rate".  We  integrate  Equations  (41)  -  (46)  by  using  the 
Euler  integration  method  in  a  function  called  "update-velocity".  This  function  will 
produce  the  translational  and  rotational  velocities  in  body  coordinates  which  will  be  used 
to  determine  the  vehicle  position  in  earth  coordinates.  The  functions  just  mentioned  as 
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well  as  all  the  rest  of  the  LISP  source  code  will  be  discussed  in  Section  D.  This  will  be 
done  by  coordinate  transformations  which  are  discussed  in  the  next  section.  The  results 
are  also  used  to  calculate  the  required  thruster  forces  necessary  to  maneuver  the  vehicle 
to  the  desired  posture. 

C.  TRANSFORMATIONS 

To  describe  three-dimensional  objects,  a  three  dimensional  coordinate  system  is 
needed.  A  three-dimensional  Cartesian  system  is  commonly  used  due  to  the  ease  at  which 
it  can  be  manipulated  using  matrix  algebra.  For  this  thesis  the  object  is  constructed  with 
polygons  (Figure  7). 

This  section  discusses  the  use  of  the  Homogeneous  Transformations,  Euler  Angle 
Transformations  and  Non-orthogonal  transformations  for  the  three  dimensional  simulation 
of  the  NPS  Phoenix  AUV  in  response  to  the  hover  mode  autopilot.  The  purpose, 
application,  and  implementation  of  each  are  discussed  beginning  with  the  Homogeneous 
transformation. 

1.  Homogeneous  Transformation 

The  Homogeneous  Transformation  is  a  homogeneous  coordinate  system  which 
includes  both  rotation  and  translation  values  (Paul,  1981).  Rotation  refers  to  pivoting  the 
object  about  its  axes  where  azimuth  is  the  angle  about  the  z  axis,  elevation  is  the  angle 
about  the  rotated  y  axis  and  roll  is  the  angle  about  the  rotated  x  axis.  Translation  refers 
to  moving  an  object  to  a  new  point  in  the  coordinate  system  while  maintaining  its 
orientation.  A  Homogeneous  Transformation  is  the  result  of  a  rotation  followed  by  a 
translation.  It  is  this  transformation  that  represents  general  three-dimensional  movement 
of  a  rigid  body  figme. 
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0 

1 

(47) 


The  Homogeneous  transformation  matrix  is  (McGhee,  1994)  defined  by  the  above 
equation.  Note  that  Equation  (47)  is  merely  the  three  by  three  direction  cosine  matrix 
expanded  to  a  four  by  four  matrix  where  the  last  column  represents  the  amount  of 
translation.  This  transformation  accomplishes  a  rotation  followed  by  a  translation.  The 
LISP  function  called  "homogeneous-transform"  located  in  the  file  named  "robot- 
kinematics,  cl"  performs  the  action  for  the  simulation.  Equation  (47)  is  used  to  rotate  and 
translate  each  node,  which  is  done  by  a  LISP  method  called  "transform-node-list"  in  a  file 
named  "update-functions.d".  A  node  is  a  vector  consisting  of  the  x,  y,  and  z  coordinates 
of  the  three-dimensional  coordinate  system  augmented  with  a  value  of  one  as  the  fourth 
coordinate.  Performing  the  above  actions  on  each  node  of  the  polygon  figure  produces  the 
three  dimensional  effect  of  motion.  The  values  used  in  the  homogeneous  transformation 
are  in  earth  coordinates,  which  are  obtained  from  converting  the  body  velocities  into  earth 
velocities  and  then  integrating  them.  Further  details  of  this  process  follow  in  the  next  two 
sub-sections. 

2.  Euler  Angle  Transformation 

Euler  Angle  Transformations  are  used  to  transform  vectors  between  the  earth 
coordinate  system  and  the  body  coordinate  system.  This  transformation  is  used  three  times 
in  the  simulation.  The  first  time  it  is  used  in  conjunction  with  the  Homogeneous 
Transformation,  since  it  is  the  result  of  the  rotation  about  the  axes.  The  second  time  it  is 
used,  its  inverse  is  taken  and  pre-multiplied  with  the  body  translational  velocities  to  obtain 
the  translational  rates  in  earth  coordinates  for  use  in  the  control  equations.  Finally,  the 
Euler  Angle  Transformation  matrix  is  used  to  calculate  the  forces  in  body  coordinates 
when  solving  for  the  thruster  forces. 
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In  order  to  simulate  the  dynamics  of  the  AUV,  the  Newton-Euler  equations  are 
used  as  the  mathematical  model  to  obtain  the  translational  and  rotational  body  accelerations 
which  are  integrated  to  get  the  translational  and  rotational  body  velocities.  In  actuality, 
the  acceleration  and  velocity  values  would  be  generated  by  the  inertial  navigation  system. 
In  either  case,  the  body  velocities  must  be  transformed  into  earth  velocities.  By  integrating 
the  earth  velocities,  we  get  the  position  in  earth  coordinates.  The  translational  velocities, 
or  any  vector  for  that  matter,  can  be  converted  between  body  coordinates  and  earth 
coordinates  using  the  direction  cosine  matrix  as  a  function  of  Euler  angles;  this  matrix  has 
been  defined  by  Equation  (34).  Thus,  the  relationship  E=RB,  where  E  represents  any 
three  by  one  earth  vector,  R  is  the  direction  cosine  matrix  previously  defined,  and  B  is  any 
three  by  one  body  vector,  is  used  to  convert  the  translational  body  velocities  into  earth 
velocities  (Frank,  1969)  resulting  in  the  following. 


X 

cosi|;cosO  cos!|;s]n6sin(^>siiii|fC05if>  cosi|rsm6co5i|>^smi|rsia4> 

U 

y 

m 

sinilfCosO  cosi|jcos(^«siii4rsiii0sm«|>  'Cosi|rsini^^sini|fsindcos(^ 

V 

z 

-sin0  cosOsini|>  co56cos<|> 

w 

Since  the  direction  cosine  matrix  is  computed  as  part  of  the  homogeneous  transformation 
matrix,  it  is  extracted  via  a  LISP  method  called  "extract-R-matrbc".  This  action  avoids 
additional  computations  when  determining  the  R  matrix.  The  above  procedure  has  only 
transformed  the  translational  velocities,  the  next  subsection  describes  how  the  rotational 
velocities  are  transformed. 

3.  Non-orthogonal  Transfonnation 

The  Non-orthogonal  transformation  is  used  to  transform  the  body  rotational 
velocities  to  Euler  angle  rates.  This  is  necessary  for  numerical  integration  of  Euler  rates 
to  obtain  Euler  angles. 
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The  rotational  body  accelerations  that  are  obtained  from  the  Newton-Euler 
equations  are  integrated  yielding  the  rotational  body  velocities.  Since  these  vectors  are  not 
orthogonal,  a  non-orthogonal  transformation  is  required.  (Frank,  1969)  defines  the  non- 
orthogonal  transformation  matrix  as 
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Equation  (49)  provides  the  means  for  transforming  the  rotational  body  velocities  to  earth 
velocities  by  pre-multiplying  the  vector  [p  q  r]’*^  with  the  non-orthogonal  transformation 
matrix  as  shown  below. 
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The  resultant  vector  from  Equation  (50)  is  integrated  to  give  us  the  Euler  Angles  0,  <|>,  and 
7.  These  angles  are  used  in  the  Homogeneous  Transformation  matrix  as  well  as  the  Euler 
Angle  Transformation  matrbc.  They  are  also  important  in  determining  the  required  control 
torques. 

D.  LISP  CODE 

Appendix  A  contains  the  entire  LISP  source  code  for  this  thesis.  This  section 
serves  to  describe  the  purpose  of  the  essential  functions  utilized  in  the  implementation  and 
simulation  of  the  hover  mode  autopilot.  Due  to  the  complexity  of  the  code,  the  simulation 
cycle  is  best  described  using  a  flowchart  (Figure  8).  Refer  to  Table  2  for  the  symbol 
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Figure  8 


LISP  Code  Simulation  Cycle  Flowchart 
41 


definitions.  Several  details  have  been  intentionally  left  out  for  clarity  of  the  simulation 
process.  The  following  subsections  will  briefly  describe  the  functions  named  in  the 
flowchart. 

1.  "initialize-all" 

Before  the  simulation  can  run  it  must  be  initialized.  This  is  accomplished  by  the 
function  "initialize-all".  The  object  class  is  created  and  the  vehicle  parameters  (mass, 
added  mass  diagonal  terms,  buoyancy  force,  natural  frequency  and  damping  ratio)  are  set. 
Although  the  values  used  are  those  relative  to  the  NFS  Phoenix  AUV,  they  can  be  set  to 
anything.  Default  initial  conditions  and  desired  conditions  are  set  as  part  of  the 
initialization  but  can  be  changed  prior  to  actually  running  the  simulation.  Finally,  the 
windows  for  observing  the  AUV's  movements  are  initialized. 

2.  "update-velocity-growth-rate" 

This  function  uses  the  Newton-Euler  dynamic  equations  discussed  earlier  to 
determine  the  body  translational  and  rotational  velocity  growth  rates.  The  results  are  the 
values  from  the  mathematical  model  used  to  simulate  the  sensed  data. 

3.  "update-velocity" 

The  Euler  numerical  integration  method  is  used  to  obtain  the  body  translational  and 
rotational  velocities  from  the  Newton-Euler  dynamic  equations.  This  is  accomplished  by 
multiplying  the  velocity  growth  rates  with  the  initialized  time  step  and  adding  the  result 
to  the  previous  values  of  the  velocities  for  the  new  body  velocities.  In  order  to  keep  the 
error  accumulation  to  a  reasonable  value,  a  maximum  time  step  of  0.01  seconds  is  used. 

4.  "homogeneous-transform" 

The  Homogeneous  transformation  matrix  (H-matrix)  is  updated  at  each  time  step 
using  this  function.  Equation  (47)  is  implemented  using  the  Euler  angles  and  earth  fixed 
coordinates  to  calculate  the  new  Homogeneous  transformation  matrix. 
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5.  "extract-R-matrix" 

The  direction  cosine  matrix  (R-matrix)  is  extrapolated  from  the  H-matrix.  The  R- 
matrix  is  used  to  transform  the  various  vectors  back  and  forth  between  the  body  coordinate 
system  and  the  earth  fixed  coordinate  system. 

6.  "solve-xyz-dot-earth" 

This  function  uses  the  R-matrix  to  transform  the  body  translational  velocities  to 
earth  translational  rates.  These  rates  are  then  integrated  using  the  Euler  integration 
method. 

7.  "solve-eiiler-angle-rates” 

The  Euler  angle  rates  are  calculated  by  pre-multiplying  the  body  rotational  rates 
with  the  Non-orthogonal  transformation  matrix,  which  accomplished  within  this  function. 

8.  "update-position" 

Once  the  earth  fixed  translational  and  rotational  rates  have  been  determined  using 
the  previous  two  functions,  the  earth  fixed  coordinates  and  the  Euler  angles  can  be 
calculated.  This  is  done  by  using  the  Euler  numerical  integration  method.  As  in  the 
"update-velocity"  function,  the  rates  are  multiplied  by  the  time  step  and  then  added  to  the 
previous  coordinates  and  Euler  angles  to  give  the  new  position.  Also,  as  in  the  "update- 
velocity"  function,  the  time  step  used  is  0.01  seconds  to  prevent  an  excessive  error 
accumulation. 

9.  "move" 

The  H-matrix  is  applied  to  each  node  of  the  polygon  figure  using  the  calculated 
Euler  angles  and  earth  fixed  coordinates.  Once  all  the  nodes  have  been  translated  and 
rotated  to  their  new  position  the  polygon  figure  is  redrawn  by  applying  the  "take-picture" 
function. 


43 


10.  "linear-feedback" 

This  function  implements  Equations  (20)  -  (24),  the  control  equations,  to  determine 
the  forces  and  torques  in  the  earth  fixed  frame.  Since  roll  stability  is  incorporated  into  the 
physical  design  of  the  AUV,  the  torque  about  the  x  axis  is  neglected  and  assumed  to  be 
zero. 

11.  "update-thrusters" 

Thruster  forces  are  relative  to  the  body  coordinate  system;  hence,  to  use  the  earth 
fixed  coordinate  forces  they  must  be  transformed  into  body  forces.  This  is  accomplished 
by  taking  the  inverse  of  the  R-matrix,  which  happens  to  be  its  transpose  as  mentioned 
earlier,  and  pre-multiplying  it  to  the  earth  forces.  The  Euler  angles  are  used  in  the  torque 
equations  for  the  moments  about  the  y  and  z  axes.  These  body  forces  and  torques  are  then 
pre-multiplied  by  the  thruster  coefficient  matrix  to  obtain  the  required  thruster  forces. 
The  required  thruster  forces  are  compared  against  the  limits  of  the  thrusters  with  the 
function  "check-thruster-limit".  If  a  required  thruster  force  is  greater  than  its  limit,  the 
autq)ilot  simply  saturates  the  thruster  by  commanding  the  maximum  force  that  the  thruster 
is  capable  of. 

12.  "update-forces-and-torques" 

The  Newton-Euler  dynamic  equations  utilize  the  forces  and  torques  resulting  from 
the  thruster  commands  given  by  the  autopilot.  Buoyancy  effects  are  included  in 
determining  the  forces  and  moments.  This  completes  the  simulation  cycle. 

E.  SUMMARY 

The  philosophy  behind  the  gr£q)hical  simulation  of  the  AUV  has  been  presented. 
An  explanation  of  the  mathematical  model  used  to  simulate  the  AUV's  dynamics  which 
are  applied  to  the  designed  autopilot  was  discussed.  Several  transformations  and  their 
purposes  have  also  been  presented.  These  transformations  include  the  Homogeneous 
Transformation,  Euler  Angle  Transformation  and  Non-orthogonal  transformation.  Finally, 
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the  essential  cx)de  segments  for  implementing  the  autopilot  design  and  simulating  the 
responses  to  the  autopilot  both  numerically  and  graphically  have  been  presented.  The  next 
chapter  discusses  the  results  obtained  from  the  autopilot  design. 


45 


VI.  SIMULATION  RESULTS 


A.  INTRODUCTION 

Since  this  autopilot  design  has  not  been  incorporated  into  the  NPS  Phoenix  AUV, 
actual  data  from  the  vehicles  response  could  not  be  collected.  Only  the  results  of  the 
simulation  are  presented.  The  only  comparisons  discussed  are  those  between  the  initial 
design,  which  does  not  include  added  mass  effects,  and  the  second  design,  which  includes 
only  the  diagonal  terms  of  the  added  mass  effects.  As  a  means  to  compare  the  numerical 
values  produced  by  the  LISP  code  implementation  of  the  autopilot,  a  MATLAB  program 
was  developed  in  parallel;  however,  the  MATLAB  code  does  not  include  the  quadratic 
terms  of  the  Newton-Euler  dynamic  equations.  First  the  results  of  the  MATLAB  code 
implementation  of  the  autopilot  are  discussed  followed  by  the  LISP  code  results.  The 
initial  conditions  and  desired  conditions  for  the  simulations  are  listed  in  Table  3.  All 
simulations  were  run  for  300  seconds.  Figures  9  -  38  at  the  end  of  this  chapter  illustrate 
the  results. 


Table  3 

Simulation  Conditions 
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B.  MATLAB  CODE  SIMULATION  RESULTS 

The  MATLAB  source  code  developed  to  compare  against  the  LISP  source  code 
results  is  located  in  Appendix  B.  Figures  9-18  show  the  results  of  the  MATLAB 
simulation  where  the  solid  lines  represent  the  product  of  the  first  design  which  did  not 
include  the  added  mass  effects.  The  dotted  lines  indicate  the  values  calculated  including 
the  diagonal  terms  of  the  added  mass  effects.  As  can  be  seen  in  these  figures,  the  end 
results  of  both  designs  are  the  same  with  only  slight  deviations  between  the  two  designs. 
Studying  these  figures  it  can  he  seen  that  the  diagonal  terms  of  the  added  mass  effects  are 
minimal. 

C.  LISP  CODE  SIMULATION  RESULTS 

The  LISP  source  code  is  included  as  Appendix  A.  Figures  19-32  illustrate  the 
values  calculated  by  the  LISP  source  code.  The  first  design,  which  did  not  include  the 
added  mass  effects,  produced  excellent  results  and  are  depicted  as  the  solid  lines  in  Figures 
19  -28.  When  the  second  design  was  developed  and  simulated  it  performed  as  expected 
illustrated  by  the  dotted  lines  in  Figures  19  -  24,  until  a  desired  pitch  angle  other  than  zero 
radians  was  commanded.  When  a  pitch  angle  was  commanded  the  system  produced 
extraneous  results  as  can  be  seen  in  Figures  29  -  32.  Since  the  second  design  of  the 
MATLAB  code  yielded  excellent  results  for  all  test  cases  it  is  assumed  that  the  problem 
with  the  implementation  of  the  second  design  in  the  LISP  code  is  due  to  a  programming 
error.  Unfortunately,  the  bug  has  not  been  found. 

Two  differences  between  the  MATLAB  and  LISP  implementations  are  that  the 
quadratic  terms  are  neglected  in  the  MATLAB  code  and  the  thruster  limits  were  not 
incorporated  into  the  MATLAB  code.  The  latter  is  only  relevant  when  the  pitch  angle  is 
greater  than  8.5  degrees,  which  was  determined  by  studying  Figures  27  -  28  where  the 
thrusters  have  saturated.  Figures  33  -  38  show  how  closely  the  second  design  implemented 
with  the  LISP  code  follows  the  first  design  implemented  with  the  MATLAB  code.  Recall 
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that  the  first  design  in  MATLAB  does  not  include  the  quadratic  terms  of  the  Newton-Euler 
dynamic  equations  or  the  added  mass  effects,  while  the  second  design  in  LISP  includes 
both.  The  exception  being  the  second  design  in  LISP  where  the  problem  with  pitch  exists 
as  shown  in  Figures  29  -  32. 

D.  CONCLUSIONS 

The  results  of  various  simulation  runs  for  both  the  MATLAB  and  LISP  code 
implementations  of  the  hover  mode  autopilot  have  been  presented.  The  simulations  of  the 
first  design  for  both  MATLAB  and  LISP  showed  favorable  results.  Simulations  for  the 
second  design  were  also  favorable  from  the  MATLAB  code.  The  LISP  code  simulations 
for  the  second  design  were  as  expected  except  for  when  a  pitch  angle  was  commanded. 
Although  this  remains  unresolved,  a  significant  advancement  in  the  current  autopilot 
implementation  has  been  achieved.  It  is  concluded  that  due  to  the  relative  slow  speed  of 
the  AUV  during  hovering  and  short  transits  the  quadratic  terms  are  negligible,  that  is,  the 
linearized  analysis  predicts  results  well.  This  is  not  always  true.  For  example,  in  walking 
machine  postural  control  much  more  rapid  response  is  needed  to  prevent  falling  over;  that 
is,  the  quadratic  terms  are  significant  as  shown  in  Figure  5b  of  (McGhee,  1986).  In  this 
example,  linear  design  produces  stability  but  does  not  predict  quadratic  coupling  between 
attitude  and  altitude. 
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MATLAB  Thruster  Force  Results  Simulation  Run  #1 
Dotted  Line  Includes  Added  Mass  Effects  Solid  Line  Does  Not 
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MATLAB  Thruster  Force  Results  Simulation  Run  #2 
Dotted  Line  Includes  Added  Mass  Effects  Solid  Line  Does  Not 
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MATLAB  Posture  Results  Simulation  Run  #3 
Dotted  Line  Includes  Added  Mass  Effects  Solid  Line  Does  Not 
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Figure  14 


MATLAB  Thruster  Force  Results  Simulation  Run  #3 
Dotted  Line  Includes  Added  Mass  Effects  Solid  Line  Does  Not 
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Figure  15 


MATLAB  Posture  Results  Simulation  Run  #4 
Dotted  Line  Includes  Added  Mass  Effects  Solid  Line  Does  Not 
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Figure  16 

MATLAB  Thruster  Force  Results  Simulation  Run  #4 
Dotted  Line  Includes  Added  Mass  Effects  Solid  Line  Does  Not 
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Figure  17 


MATLAB  Posture  Results  Simulation  Run  #5 
Dotted  Line  Includes  Added  Mass  Effects  Solid  Line  Does  Not 


58 


Aft  Vertical  Thruster-T1 


Fwd  Vertical  Thruster-T2 


Fwd  Lateral  Thruster-T5 


Figure  18 


MATLAB  Thruster  Force  Results  Simulation  Run  #5 
Dotted  Line  Includes  Added  Mass  Effects  Solid  Line  Does  Not 
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Figure  19 


LISP  Posture  Results  Simulation  Run  #1 
Dotted  Line  Includes  Added  Mass  Effects  Solid  Line  Does  Not 
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Figure  20 


LISP  Thruster  Force  Results  Simulation  Run  #1 
Dotted  Line  Includes  Added  Mass  Effects  Solid  Line  Does  Not 
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figure  21 


LISP  Posture  Results  Simulation  Run#2 
Dotted  Line  Includes  Added  Mass  Effects  Solid  Line  Does  Not 
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LISP  Thruster  Force  Results  Simulation  Run  #2 
Dotted  Line  Includes  Added  Mass  Effects  Solid  Line  Does  Not 
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Figure  23 

LISP  Posture  Results  Simulation  Run  #3 
Dotted  Line  Includes  Added  Mass  Effects  Solid  Line  Does  Not 
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LISP  Thruster  Force  Results  Simulation  Run  #3 
Dotted  Line  Includes  Added  Mass  Effects  Solid  Line  Does  Not 
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Figure  26 


LISP  Design- 1  Thruster  Force  Results  Simulation  Run  #4 
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Figure  33 

MATLAB  Design-1  (Solid)  vs.  LISP  Design-2  (Dotted)  Posture  Results  Run#l 
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Figure  34 


MATLAB  Design-1  (Solid)  vs.  LISP  Design-2  (Dotted)  Thraster  Force  Results  Run#l 
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Figure  35 

MATLAB  Design- 1  (Solid)  vs.  LISP  Design-2  (Dotted)  Posture  Results  Run#2 
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MATLAB  Design-1  (Solid)  vs.  LISP  Design-2  (Dotted)  Thruster  Force  Results  Run  #2 
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Figure  37 


MATLAB  Design-1  (Solid)  vs.  LISP  Design-2  (Dotted)  Posture  Results  Run  ifZ 
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MATLAB  Design- 1  (Solid)  vs.  LISP  Design-2  (Dotted)  Thruster  Force  Results  Run  #3 
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Vn.  SUMMARY 


A.  SYNOPSIS 

This  thesis  presented  a  basic  design  for  a  hover  mode  autopilot  to  possibly  be 
incorporated  into  the  NPS  Phoenix  AUV.  The  autopilot  was  implemented  and  simulated 
using  Common  LISP  object  oriented  programming  language.  Initially,  the  autopilot  design 
was  developed  within  the  constraints  of  the  initial  design  boundaries.  This  first  design 
produced  favorable  results  in  controlling  five  of  the  six  degrees  of  freedom.  After 
successfully  implementing  and  simulating  the  initial  design  an  additional  factor  of  reality 
was  added.  This  additional  factor  was  the  diagonal  terms  of  the  added  mass  effects.  In 
the  case  of  the  second  design,  favorable  results  were  obtained  when  controlling  surge, 
sway,  heave,  and  yaw.  When  an  attempt  to  include  the  control  of  pitch  was  made,  the 
autopilot  produced  erroneous  results.  However,  this  problem  is  assumed  to  be  caused  by 
an  error  in  the  LISP  source  code.  This  assumption  is  based  on  the  fact  that  a  MATLAB 
implementation  of  the  modified  autopilot  design  provided  meaningful  results.  Although 
this  problem  has  not  been  solved,  the  ability  to  control  the  four  degrees  of  freedom 
mentioned  above  simultaneously  is  a  significant  improvement  over  the  current  hover  mode 
autopilot  implemented  in  the  vehicle  where  these  actions  are  performed  separately. 

B.  RECOMMENDATIONS  FOR  FURTHER  STUDY 

This  thesis  investigated  the  design  and  simulation  of  a  hover  mode  autopilot  using 
LISP.  The  control  design  proved  to  be  favorable  with  some  exceptions.  As  a  result, 
further  investigation  into  the  control  design  presented  is  possible. 

The  fact  that  the  second  controller  design  may  not  have  been  properly  implemented 
with  the  LISP  code  provides  the  opportunity  for  a  detailed  investigation  of  the  source  code. 
BCnowledge  in  programming  with  LISP  as  well  as  an  adequate  amount  control  theory  is  an 
essential  requirement  to  try  and  correct  the  problem  with  controlling  pitch. 
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Further  investigation  of  the  autopilot  controller  design  is  also  a  potential  source  of 
study.  The  controller  design  in  and  of  itself  is  substantial.  Expanding  Chapter  IV  to 
eliminate  many  of  the  boundaries  as  set  forth  in  Chapter  III  is  a  worthy  course  of  action. 
The  reason  for  this  action  is  that  it  may  provide  a  more  realistic  model  which  would 
produce  simulated  results  that  are  closer  to  the  actual  vehicle  responses. 
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APPENDIX  A.  LISP  SOURCE  CODE 


This  appendix  contains  the  all  the  LISP  code  for  creating  the  Phoenix  AUV 
polygon  model  for  which  the  autopilot  design  is  applied. 

A.  OPERATING  INSTRUCTIONS 

In  order  to  run  the  simulation  the  following  instructions  are  provided.  They 
assume  that  the  required  files  are  installed  in  the  current  directory  and  the  user  has  access 
to  and  a  basic  familiarity  of  using  Common  LISP. 

-  At  the  prompt  call  up  LISP. 

-  Load  the  required  files  by  typing  (load  Toad-fasl"). 

-  Initialize  the  simulation  by  typing  (initialize-all).  This  will  set  all  parameters  to 
default  values.  If  different  values  are  desired,  one  only  need  to  understand  the  file 
"initialization,  cl". 

-  Run  the  simulation  by  typing  (autopilot  dt  stop-time),  where  dt  is  the  desired 
time  step  and  stop-time  is  the  total  run  time  in  seconds.  Note  that  using  0.01  for  dt 
provides  the  best  results. 

After  following  the  above  instructions  two  windows  will  open  and  provide  an  overhead 
view  as  well  as  a  view  side  view.  The  numerical  results  for  the  posture  values  will  be 
displayed  in  the  window  from  which  LISP  was  started  and  they  will  also  be  routed  to  a  file 
named  "auvsim.dat"  for  later  analysis. 
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B.  AUV-RIGID-BODY.CL 


This  file  was  originally  written  by  Dr.  McGhee  to  create  a  generic  rigid-body.  It 
has  been  modified  by  the  author  to  create  the  Phoenix  AUV  rigid-body. 


(defclass  rigid-body 

0 

((posture  ;Tbe  six-vector  (psi  theta  phi  x  y  z)  in  world  coordinates. 

:initfonn  ’(000000) 

:imtarg  rposture 
raccessor  posture) 

(velocity  ;The  six-vector  (u  v  w  p  q  r)  in  body  coordinates. 

:imtfonn  '(000000) 
rinitarg  rvelocity 
raccessor  velocity) 

(velocity-growth-rate  ;The  vector  (u-dot  v-dot  w-dot  p-dot  q-dot  r-dot). 
rinitform  '(000000) 
raccessor  velocity-growth-rate) 

(forces-and-torques  ;The  vector  (Fx  Fy  Fz  L  M  N)  in  body  coordinates, 
rinitfonn  '(000000) 
raccessor  forces-and-torques) 

(moments-of-inertia  ;The  vector  (Tx  ly  Iz)  in  princ^al  axis  coordinates, 
rinitform  ’(111) 
rinitarg  rmoments-of-inertia 
raccessor  moments-of-inertia) 

(mass 

rinitform  '(111) 
rinitarg  rmass 
raccessor  mass) 

(node-list  ;(x  y  z  1)  in  body  coord  for  each  node.  Starts  with  (0  0  01). 
rinitform  ’((0  0  0  1)  (4  0  0  1)  (2  0  0  1)  (-4  0  0  1)  (-5  0  -2  1) 

(-6  -1.5  -2  1)  (-6  1.5  -2  1)  (-2  6  -2  1)  (-2  -6  -2  1)) 
rinitarg  mode-list 
raccessor  node-list) 

(polygon-list 

rinitform  '((1  2  3  4  5)  (4  6)  (7  2  8)) 
rinitarg  rpolygon-list 
raccessor  polygon-list) 

(transformed-node-list  ;(x  y  z  1)  in  earth  coord  for  each  node  in  node-list, 
raccessor  transformed-node-list) 

(H-matrix 

rinitform  (unit-matrix  4) 
raccessor  H-matrix) 

(delta-t 

raccessor  delta-t))) 
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;;  define  AUV-body  subclass  of  rigid-body 
(defclass  AUV-body  (rigid-body) 

((node-list 

rinitfonn  '((0  0  0  1)  (-3.58  -.67  0  1)  (-2.25  -.67  -.42  1)  (2.54  -.67  -.42  1) 

(2.75  -.63  -.4  1)  (3  -.58  -.35  1)  (3.17  -.5  -.29  1)  (3.38  -.38  -.21  1) 

(3.58  -.21  -.08  1)  (3.58  -.21  .08  1)  (3.38  -.38  .21  1)  (3.17  -.5  .29  1) 

(3  -.58  .34  1)  (2.75  -.63  .4  1)  (2.54  -.67  .42  1)  (-2.25  -.67  .42  1) 

(-3.58  .67  0  1)  (-2.25  .67  -.42  1)  (2.54  .67  -.42  1)  (2.75  .63  -.4  1) 

(3  .58  -.35  1)  (3.17  .5  -.29  1)  (3.38  .38  -.21  1)  (3.58  .21  -.08  1) 

(3.58  .21  .08  1)  (3.38  .38  .21  1)  (3.17  .5  .29  1)  (3  .58  .34  1) 

(2.75  .63  .4  1)  (2.54  .67  .42  1)  (-2.25  .67  .42  1) 

(-2.67  0  -.42  1)  (-2.46  0  -.84  1)  (-2.25  0  -.84  1)  (-2.25  0  -.42  1) 

(-2.67  0  .42  1)  (-2.46  0  .84  1)  (-2.25  0  .84  1)  (-2.25  0  .42  1) 

(-2.67  -.67  0  1)  (-2.46  -1.09  0  1)  (-2.25  -1.09  0  1)  (-2.25  -.67  0  1) 
(-2.67  .67  0  1)  (-2.46  1.09  0  1)  (-2.25  1.09  0  1)  (-2.25  .67  0  1) 

(2.54  0  -.42  1)  (2.54  0  -.84  1)  (2.33  0  -.84  1)  (2.12  0  -.42  1) 

(2.54  0  .42  1)  (2.54  0  .84  1)  (2.33  0  .84  1)  (2.12  0  .42  1) 

(2.54  -.67  0  1)  (2.54  -1.09  0  1)  (2.33  -1.09  0  1)  (2.12  -.67  0  1) 

(2.54  .67  0  1)  (2.54  1.09  0  1)  (2.33  1.09  0  1)  (2.12  .67  0  1))) 
(polygon-list 

rinitfonn  ’((1  2  3  4  5  6  7  8  9  10  11  12  13  14  15) 

(16  17  18  19  20  21  22  23  24  25  26  27  28  29  30)  (8  9  23  24)  (1  16) 

;  (2  17)  (3  18)  (14  29)  (15  30) 

(31  32  33  34)  (35  36  37  38)  (39  40  41  42) 

(43  44  45  46)  (47  48  49  50)  (51  52  53  54)  (55  56  57  58)  (59  60  61  62))) 
(thrust-com  ;;  tbiusters  T1  T2  T3  T4  T5 

:initform  ’(0  0  0  0  0) 

:initarg  :thrust-com 
raccessor  thrust-com) 

(gains 

;;  Kx  Rsdot  Ky  Kydot  Kz  Kzdot  Kpsi  Kpsidot  Ktheta  Kthetadot  Kphi  K^hidot 
rinitfonn  '(111111111111) 
rinitarg  rgains 
raccessor  gains) 

(desired-posture  ;;  xo  yo  zo  psio  thetao  phio 

rinitfonn  ’(000000) 
rinitarg  rdesired-posture 
raccessor  desired-posture) 

posture-rates  ;;  xdot  ydot  zdot  psidot  thetadot  phidot 

rinitform  '(000000) 
rinitarg  rposture-rates 
raccessor  posture-rates) 

(forces-earth-coord  ;;  Fxe  Fye  Fze  Tpsi  Ttheta  Tphi 

rinitform  ’(000000) 

rinitarg  rforces-earth-coord 

raccessor  forces-earth-coord) 

physical-char  ;;  m  I  Fb  Fw 

rinitform  ’(1  111) 


85 


rinitarg  rphysical-char 
;accessor  physical-char) 
(R-matrix 

rinitfoim  (unit-matrix  3) 
raccessor  R-matrix) 
(added-m-coeff 
rinitfoim  '(0  0  0) 

:accessor  added-m-coefif) 
(added-I-coeff 
linitfotm  '(0  0  0) 
raccessor  added-I-coeff) 

))  ;close  AUV-body  subclass 


C.  B2E-TRANSFORMAT10NS.CL 

This  file  was  written  by  the  author  to  transform  body  translational  and  rotational 
velocities  to  earth  translational  velocities  and  Euler  angle  rates. 


;;  iunctioii  to  solve  for  psi-dot,  theta-dot,  phi-dot  (Euler  angle  rates) 
(dehnethod  solve-euler-angk-rates  ((body  rigid-body)) 
(multiple-value-bind  (u  v  w  p  q  r  psi  theta  phi) 

(values-list  (append  (velocity  body)  (&stn  3  posture  body)))) 
(list 

(+  (*  (sec  theta)  (sin  phi)  q) 

(*  (sec  theta)  (cos  phi)  r)) 

(-  (*  (cos  phi)  q) 

(*  (sin  phi)  r)) 

(+  P 

(*  (tan  theta)  (sin  phi)  q) 

(*  (tan  theta)  (cos  phi)  r))))) 

;;  function  to  solve  for  x-dot,  y-dot,  z-dot  in  earth  coordinates 
(dehnethod  solve-xyz-dot-earth  ((body  rigid-body)) 

(post-mult^ly  (R-matrix  body)  (firstn  3  (velocity  body)))) 
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D.  INITIALIZATION.CL 


This  code  was  written  by  the  author  to  initialize  the  simulation  by  setting  the 
parameters  and  default  initial  conditions. 

;;  function  to  initialize  everything  to  default  values 
(definethod  initialize-all  0 
(setf  sub  (make-instance  ’AUV-body)) 

(set-parameters  12.0117  1.0650  12.9541  35.2744  2.7  42  45 

4.8330  125.8594  9.4646  389  .04  .8  sub) 

(set-init-cond  000000000000  sub) 

(set-desired-cond  15  10  5  pi  0  0  sub) 

(init-cameras) 

);  close  initialize-all 

;;  function  to  reset  to  default  values  and  clear  existing  cameras 
(definethod  reset  0 

(setf  sub  (make-instance  ’AUV-body)) 

(set-parameters  12.0117  1.0650  12.9541  35.2744  2.7  42  45 

4.8330  125.8594  9.4646  389  .04  .8  sub) 

(set-init-cond  000000000000  sub) 

(set-desired-cond  15  10  5  pi  0  0  sub) 

(dear-cameras) 

);  close  reset 


;;  function  to  set  physical  parameters  (mass,  inertia,  etc.) 

(definethod  set-parameters  (mass  xudot  yvdot  zwdot  Ix  ly  Iz 

kpdot  mqdot  nrdot  Fb  w  z  (body  rigid-body)) 

(setf  (added-m-coeff  body)  (list  xudot  yvdot  zwdot)) 

(setf  (added-l-coeff  body)  (list  Iqxiot  mqdot  nidot)) 

(setf  (moments-of-inertia  body) 

(vector-add  (list  Ix  ly  Iz)  (added-I-coeff  body))) 

(setf  (mass  body) 

(vector-add  (list  mass  mass  mass)  (added-m-coeff  body))) 

(setf  m  (average-vector(vector-add  (list  mass  mass  mass)  (added-m-coeff  body)) )) 

(setf  I  (average-vector  (cdr  (vector-add  (list  Ix  ly  Iz)  (added-I-coeff  body)) ))) 

(setf  (forces-and-torques  body)  (list  0  0  (-  (*  *gravity*  (-1-  zwdot  mass)))  0  0  0)) 

(setf  physical-char  body)  (list  m  I  (-1-  (*  *gravity*  zwdot)  Fb)  (*  *gravity*  (-f-  zwdot  mass)))) 


(setf  (gains  body) 

(list  (*  m  (ejq)t  w  2))  ;Kx 

(*  2  z  w  m)  ;Kxdot 
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(*  m  (ejq)t  w  2)) 

;Ky 

(*  2  z  w  m) 

;Kydot 

(*  m  (e3q)t  w  2)) 

;Kz 

(*  2  z  w  m) 

;Kzdot 

(*  I  (expt  w  2)) 

;Kpsi 

(*  2  z  w  I) 

;Kpsidot 

(*  I  (ejqtt  w  2)) 

;Ktheta 

(*  2  z  w  I) 

;Kthetadot 

(*  I  (expt  w  2)) 

;Kphi 

(*  2  z  w  I) )) 

;Kphidot 

);close  set-parameters 


;;  function  to  set  initial  conditions  for  AUV 
(definethod  set-init-cond 

(x  xdot  y  ydot  z  zdot  psi  psidot  theta  thetadot  phi  phidot  (body  rigid-body)) 
(setf  (posture  body)  (list  psi  theta  phi  x  y  z)) 

(setf  (posture-rates  body)  (list  psidot  thetadot  phidot  xdot  ydot  zdot )) 
);close  set-init-cond 


;;  function  to  iqput  desired  condtions  (position,  angle  etc.)  for  AUV 
(definethod  set-desired-cond  (xo  yo  zo  psio  thetao  phio  (body  rigid-body)) 
(setf  (desired-posture  body)  (list  xo  yo  zo  psio  thetao  phio)) 

);  close  set-desired-cond 


;;definethod  to  initialize  required  cameras 
(definethod  init-cameras  0 
(setf  camera-3  (make-instance  'strobe-camera)) 
(setf  camera-2  (make-instance  ’strobe-camera)) 
(move  camera-3  0  (/  pi  -2)  0  0  0  -25) 

(move  camera-2  (-  (/  pi  2  ))  0  0  0  25  0) 

);close  init-cameras 


;;definethod  to  take  a  round  of  pictures 
(definethod  grp-picture  0 

(take-picture  camera-2  sub) 
(take-picture  camera-3  sub) 
);close  grp-picture 
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E.  MISC-FUNCTIONS.CL 


This  file  contains  functions  that  the  author  found  necessary  to  create. 

;;Misc  functions 

(defun  eleventh  (1)  (nth  10 1)) 

(defim  twelfth  a)  (nth  111)) 

(defconstant  *gravity*  32.2185) 

(defim  sec  (angle)  (/ 1  (cos  angle))) 


(definethod  dear-cameras  0- 
(cwrclear  (camera-window  camera-2)) 

(cwxlear  (camera-window  camera-3)) 

);  close  dear-cameras 

;;  hmction  to  display  posture  values  on  screen  during  execution 
(definethod  show-vdues  ((body  rigid-body)) 

(setf  X  (posture  body)) 

(format  t  ”-%  ~6,3f  ~6,3f  ~6,3f  ~6,3f  ~6,3f  ~6,3f " 
(first  x)  (second  x)  (third  x)  (fourth  x)  (fifth  x)  (sixth  x)) 

);  close  show-vahies 

(defim  average-vector  (vector) 

(setf  sum  0  count  0) 

(dolist  (element  vector  average) 

(setf  sum  (-(-  sum  element)) 

(setf  count  (-1-  count  1)) 

(setf  average  (/  sum  count)))) 
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F.  ROBOT-KINEMATICS.CL 


This  is  original  code  was  written  by  Dr.  McGhee.  It  contains  the  necessary 
functions  associated  for  kinematic  modeling. 

(defun  transpose  (matrix)  ;A  matrix  is  a  list  of  row  vectors. 

(cond  ((null  (cdr  matrix))  (mapcar  'list  (car  matrix))) 

(t  (m^ar  'cons  (car  matrix)  (transpose  (cdr  matrix)))))) 

(defim  dot-product  (vector-1  vector-2)  ;A  vector  is  a  list  of  numerical  atoms. 

(apply  '-h  (mapcar  ’*  vector- 1  vector-2))) 

(defun  vector-magnitude  (vector)  (sqrt  (dot-product  vector  vector))) 

(defim  post-mukiply  (matrix  vector) 

(cond  ((null  (rest  matrix))  (list  (dot-product  (first  matrix)  vector))) 

(t  (cons  (dot-product  (first  matrix)  vector) 

(post-mnltiply  (rest  matrix)  vector))))) 

(defim  pre-mult5)ly  (vector  matrix) 

(post-multiply  (transpose  matrix)  vector)) 

(defim  matrix-multiply  (A  B)  ;A  and  B  are  conformable  matrices. 

(cond  ((null  (cdr  A))  (list  ^re-mult^ly  (car  A)  B))) 

(t  (cons  (pre-mnlt^ly  (car  A)  B)  (matrix-mult^ly  (cdr  A)  B))))) 

(defim  cbam-mult^ly  (L)  ;L  is  a  list  of  names  of  conformable  matrices. 

(cond  ((null  (cddr  L))  (matrix-multiply  (eval  (car  L))  (eval  (cadr  L)))) 

(t  (matrix-multiply  (eval  (car  L))  (chain-mult^ly  (cdr  L)))))) 

(defim  cycle-left  (matrix)  (m^car  ’row-cycle-left  matrix)) 

(defim  row-cycle-left  (row)  (append  (cdr  row)  (list  (car  row)))) 

(defim  cycle-up  (matrix)  (append  (cdr  matrix)  (list  (car  matrix)))) 

(defim  unit-vector  (one-cohmm  length)  ;Column  count  starts  at  1 . 

(do  ((n  length  (1-  n)) 

(vector  nil  (cons  (cond  ((=  one-colunm  n)  1)  (t  0))  vector))) 

((zerop  n)  vector))) 

(defim  unit-matrix  (size) 

(do  ((row-nuriiber  size  (1-  row-number)) 

(I  nil  (cons  (unit-vector  row-number  size)  I))) 

((zerop  row-rrurriber)  I))) 
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(deftm  concat-matrix  (A  B)  ;A  and  B  are  matrices  with  equal  number  of  rows, 
(cond  ((null  A)  B) 

(t  (cons  (append  (car  A)  (car  B))  (concat-matrix  (cdr  A)  (cdr  B)))))) 
(defun  augment  (matrix) 

(concat-matrix  matrix  (unit-matrix  (length  matrix)))) 

(defun  normalize-row  (row)  (scalar-mult^ly  (/ 1.0  (car  row))  row)) 

(defun  scalar-multiply  (scalar  vector) 

(cond  ((null  vector)  nil) 

(t  (cons  (*  scalar  (car  vector)) 

(scalar-multiply  scalar  (cdr  vector)))))) 

(defun  solve-first-column  (matrix)  ;Reduces  first  column  to  (1  0  ...  0). 

(do*  ((remaining-row-list  matrix  (rest  remaining-row-hst)) 

(first-row  (normalize-row  (first  matrix))) 

(answer  (list  first-row) 

(cons  (vector-add  (first  remaining-row-list) 

(scalar-multiply  (-  (caar  remaining-row-list)) 
first-row)) 

answer))) 

((null  (rest  remaining-row-list))  (reverse  answer)))) 

(defim  vector-add  (vector-1  vector-2)  (mapcar  ’  +  vector-1  vector-2)) 

(defun  vector-subtract  (vector-1  vector-2)  (mapcar  '-  vector-1  vector-2)) 

(defim  first-square  (matrix)  ;Retuins  leftmost  square  matrix  from  argument. 

(do  ((size  (length  matrix)) 

(remainder  matrix  (rest  remainder)) 

(answer  nil  (cons  (firstn  size  (first  remainder))  answer))) 

((null  remainder)  (reverse  answer)))) 

(defun  firstn  (n  list) 

(cond  ((zerop  n)  nil) 

(t  (cons  (first  list)  (firstn  (1-  n)  (rest  list)))))) 

(defim  max-car-firstn  (n  list) 

(append  (max-car-first  (firstn  n  list))  (nthcdr  n  list))) 

(defun  matrix-inverse  (M) 

(do  ((Ml  (max-car-first  (augment  M)) 

(cond  ((null  Ml)  nil) 

(t  (max-car-firstn  n  (cycle-left  (cycle-iq)  Ml)))))) 
(n(l-aengthM))(l-n))) 

((or  (minusp  n)  (null  Ml))  (cond  ((null  Ml)  nil)  (t  (first-square  Ml)))) 

(setq  Ml  (cond  ((zerop  (caar  Ml))  nil)  (t  (solve-first-column  Ml)))))) 
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(deiim  max-car-first  (L)  ;L  is  a  list  of  lists.  This  function  finds  list  with 
(cond  ((null  (cdr  L))  L)  '.largest  car  and  moves  it  to  head  of  list  of  lists. 

(t  (if  (>  (abs  (caar  L))  (abs  (caar  (max-car-first  (cdr  L)))))  L 
(append  (max-car-first  (cdr  L))  (list  (car  L))))))) 

(defun  dh-matrix  (cosrotate  sinrotate  costwist  sintwist  length  translate) 

(list  (list  cosrotate  (-  (*  costwist  sinrotate)) 

(*  sintwist  sinrotate)  (*  length  cosrotate)) 

(Ust  sinrotate  (*  costwist  cosrotate) 

(-  (*  sintwist  cosrotate))  (*  length  sinrotate)) 

(list  0.  sintwist  costwist  translate)  (list  0.  0.  0.  1.))) 

(defim  homogeneous-transform  (azimuth  elevation  roll  x  y  z) 
(rotation-and-translation  (sin  azimuth)  (cos  azimuth)  (sin  elevation) 

(cos  elevation)  (sin  roll)  (cos  roll)  x  y  z)) 

(defim  rotation-and-translation  (^si  cpsi  sth  cth  sphi  cphi  x  y  z) 

(list  (list  (*  q)si  cth)  (-  (*  cpsi  sth  sphi)  (*  spsi  cphi)) 

(-f  (*  q)si  sth  q)hi)  (*  spsi  sphi))  x) 

(list  (*  ^si  cth)  (-!-(*  cpsi  cphi)  (*  spsi  sth  sphi)) 

(-  (*  spsi  sth  q)hi)  (*  cpsi  q)hi))  y) 

(list  (-  sth)  (*  cth  sphi)  (*  cth  q)hi)  z) 

(list  0.  0.  0.  1.))) 

(defim  inverse-H  (H)  ;H  is  a  4x4  homogeneous  transformation  matrix, 
(let*  ((minus-P  (list  (-  (fourth  (first  H))) 

(-  (fou^  (second  H))) 

(-  (fourth  (third  H))))) 

(inverse-R  (transpose  (first-square  (reverse  (rest  (reverse  H)))))) 
(inverse-P  ^ost-mnk^ly  inverse-R  minus-P))) 

(append  (concat-matrix  inverse-R  (transpose  (list  inverse-P))) 

(list  (list  0  0  0  1))))) 

(setf  X  ’((1  2)  (3  4))) 


93 


G.  RUN-SBMULATION.CL 


This  code  was  written  by  the  author.  It  is  the  top  layer  in  the  simulation  cycle. 

;;  AUV  autopilot  simulation  using  euler  solution  to  linear  equations 
(delinethod  autopilot  (time-step  halt-time) 

(setf  (delta-t  sub)  time-step) 

(tplate-iigid-body  sub) 

(gip-picture) 

(with-open-file  (output-stream  "auvsim.dat"  rdirection  :ouq)ut) 

(do( 

(time  0  (+  time  time-step)) 

(sec-count  0  (+  sec-count  time-step)) 

(sec-count2  0  (+  sec-count2  time-step))) 

((>  time  halt-time)  ’done) 

(linear-feedback  sub) 

(iqxiate-thrusters  sub) 

(check-thruster-limit  sub) 

(iplate-forces-and-torques  sub) 

(iqxlate-rigid-body  sub) 

(when(>  sec-count  10) 

(gtp-picture) 

(setf  sec-count  0) 

);  close  when 

(show-values  sob) 

(when  (>  =  sec-count2  1) 

(setf  X  (posture  sub)) 

(setf  f  (thrust-com  sub)) 

(setf  V  (posture-rates  sub)) 

(format  output-stream  ”  ~  %  ~6,3f  ~6,3f  ~6,3f  ~6,3f  ~6,3f  ~6,3f  ~6,3f  ~6,3f  ~6,3f  ~6,3f 
~6,3f  ~6,3f  ~6,3f  ~6,3f  ~6,3f  ~6,3f  -6,3^ 

(first  x)  (second  x)  (third  x)  (fourth  x)  (fifth  x)  (sixth  x) 

(first  v)  (second  v)  (third  v)  (fourth  v)  (fifth  v)  (sixth  v) 

(first  f)  (second  f)  (third  f)  (fourth  f)  (fifth  f)) 

(setf  sec-count2  0) 

);  close  when 
);close  do 

);close  with-open  auvsim.dat 
);close  autopilot 
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H.  UPDATE-FUNCTIONS.CL 


This  code  contains  functions  written  by  Dr.  McGhee  and  the  author.  It  contains 
all  the  functions  described  in  Chapter  V. 


(defmethod  iqxlate-rigid-body  ((body  rigid-body))  ;Euler  integration. 

(let  ((delta-t  (delta-t  body))) 

(update-posture  body) 

(update-velocity  body  delta-t) 

(iqjdate-velodty-growfli-rate  body) 

(iqxlate-posture-rates  body)) 

);close  iq)date-rigid-body 

;;  function  using  Newton-Euler  equations 

(definethod  update-velocity-growth-rate  ((body  rigid-body)) 

(setf  (velocity-growth-rate  body)  ;Assumes  principal  axis  coordinates  with 
(multiple-value-bind  ;origin  at  center  of  gravity  of  body. 

(Fx  FyFzLMN  uvwpqr  Ixlylzmxmy  mz)  ;Declares  local  variables. 

(values-list  ;Values  assign^. 

(append  (forces-and-torques  body)  (velocity  body)  (moments-of-inertia  body)  (mass  body))) 
(list  (-f  (*  V  r)  (*  -1  w  q)  (/  Fx  mx) 

(*  *gravity*  (first  (third  (H-matrix  body))))) 

(+  (*  w  p)  (*  -1  u  r)  (/  Fy  my) 

(*  *gravity*  (second  (third  (H-matrix  body))))) 

(+  (*  u  q)  (*  -1  V  p)  (/  Fz  mz) 

(*  *gravity*  (third  (third  (H-matrix  body))))) 

(/(-H(*(-IyIz)qr)L)Ix) 

(/(+  (*(-IzIx)rp)M)Iy) 

(/  (-1-  (*  (-  Ix  ly)  p  q)  N)  Iz))))  ;Value  returned. 

);  close  update-velocity-growth-rate 

(defmethod  update-velocity  ((body  rigid-body)  delta-t) 

(setf  (velocity  body) 

(vector-add  (velocity  body) 

(scalar-mult^ly  delta-t  (velocity-growth-rate  body)))) 

);  close  vpdate-velocity 

(definethod  update-posture  ((body  rigid-body)) 

(move  body 
(first  (posture  body)) 

(second  (posture  body)) 

(third  ^sture  body)) 

(fourth  ^sture  body)) 

(fifth  (posture  body)) 

(sixth  ^sture  body))) 

);  close  ipdate-posture 
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(definethod  move  ((body  rigid-body)  azimuth  elevation  roll  x  y  z) 
(setf  (H-matiix  body) 

(homogeneous-transform  azimuth  elevation  roll  x  y  z)) 
(setf  (R-matrix  body)  (extract-R-matrix  (H-matrix  body))) 
(transform-node-list  body) 

(update-position  body) 

);  close  move 


(dehnethod  transform-node-list  ((body  rigid-body)) 

(setf  (transformed-node-hst  body) 

(mapcar  #’ (lambda  (node-location) 

(post-muh^ly  (H-matrix  body)  node-location)) 

(node-list  body)))) 

(definethod  update-position  ((body  rigid-body)) 

(setf  ^sture  body)  (vector-add  posture  body)  (scalar-multiply  (delta-t  body) 
posture-rates  body)))) 

);  close  update-posidon 


(definethod  rqxiate-posture-rates  ((body  rigid-body)) 

(setf  posture-rates  body) 

(append  (solve-euler-angle-rates  body)  (solve-xyz-dot-earth  body))) 
);close  ipdate-posture-rates 


"flmctionto  determine  Fxe,  Fye,  Fze,  Tphi,  Ttheta,  Tpsi 
(definethod  linear-feedback  (pody  rigid-body)) 

(setf  (forces-earth-coord  body) 

(multiple-value-bind 

psi  theta  phi  x  y  z  xo  yo  zo  psio  thetao  phio  Kx  Kxdot  Ky  Kydot  Kz  Kzdot 
Kpsi  ^sidot  Ktheta  Kthetadot  ^hi  i^hidot  psidot  thetadot  phidot  xdot  ydot 
zdot  Fb  Fw) 

(values-list  (qjpend  posture  body)  (desired-posture  body)  (gains  body) 
posture-rates  body)  (nthcdr  2  physical-char  body)))) 

(Ust 

(-(-)-  (*  Kx  (-  X  xo))  (*  Kxdot  xdot))) 

(-(+(*  Ky  (-  y  yo))  (*  Kydot  ydot))) 

(-  Fb  Fw  (^-  (*  Kz  (-  z  zo))  (*  Kzdot  zdot))) 

0 

(-  (*  Fb  .04  (sin  theta))  (-t-  (*  Ktheta  (-  theta  thetao))  (*  Kthetadot  thetadot))) 
(-(-)-  (*  Kpsi  (-  psi  psio))  (*  Kpsidot  psidot)))))) 

);  close  linear-feedback 

"fimction  to  extract  the  R-matrix  from  the  H-matrix 
(definethod  extract-R-matrix  (H-matrix) 

(hst  (firsm  3  (first  H-matrix)) 

(fiirsm  3  (second  H-matrix)) 
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(firstn  3  (third  H-matrix))) 

);  close  extract-R-matrix 

;;  ftmction  to  update  forces  and  moments  in  body  coordinates 
(deiinethod  update-forces-and-torques  ((body  rigid-body)) 

(setf  (forces-and-torques  body) 

(muh^le-value-bind 

(T1  T2  T3  T4  T5  Fb  psi  theta  phi  I^hidot  phidot  u  v  w  p  q  r) 

(values-list  (append  (thrust-com  body)  (list  (third  (physical-char  body))) 

(firstn  3  (posture  body))  ^st  (twelfth  (gains  body))  (third  (posture-rates  body))) 
(velocity  body))) 

(list 

(-t-T3(*Fb  (sin  theta))) 

(-1-  T4  T5  (-  (*  Fb  (cos  theta)  (sin  phi)))) 

(+  T1  T2  (-  (*  Fb  (cos  theta)  (cos  phi))) ) 

(-  (*  T5  .04)  (*  T4  .04)  (*  Kphidot  p)(*  Fb  .04  (sin  phi))) 

(-  (*  T1  1.23)  (*  T2  1.44)  (*  T3  .04)  (*  Fb  .04  (sin  theta))) 

(-  (*  T5  1.94)  (*  T4  1.73))))) 

);  close  update-forces-and-torques 

;;  Thruster  force  calculations 

(definethod  update-thrusters  ((body  rigid-body)) 

(setf  thrust-coeff-matrix  '((0  0  1  0  0)  (0  0  0  1  1)  (1  1  0  0  0)  (1.23  -1.44  -.04  0  0) 

(0  0  0-1.73  1.94))) 

(setf  A  (append 

(i)ost-mult^ly  (transpose  (R-matrix  body))  (firstn  3  (forces-earth-coord  body))) 
(nthcdr  4  (forces-earth-coord  body)))) 

(setf  (thrust-com  body)  (post-mnltiply  (matrix-inverse  thrust-coeff-matrix)  A)) 

)  ;close  update-thrusters 

; ;  ftmction  to  check  if  thrusters  are  withiu  limits 
(definethod  check-thruster-limit  ((body  rigid-body)) 

(setf  T3  (third  (thrust-com  body))) 

(if  (>  (abs  T3)  10) 

(setf  T3  (*  10  (/  T3  (abs  T3))))) 

(setf  T1245  (^^nd  (firstn  2  (thrust-com  body))  (nthcdr  3  (thrust-com  body)))) 

(setf  final-t  nil) 

(setfT1245 

(dolist  (thrust  T1245  final-t) 

(if  (>  (abs  thrust)  2) 

(setf  final-t  (^pend  final-t  (list  (*  2  (/  thmst  (abs  thrust)))))) 

(setf  final-t  (append  final-t  (list  thrust)))))) 

(setf  (thrust-com  body)  (^end  (firstn  2  T1245)  ^t  T3)  (nthcdr  2  T1245))) 

);  close  check-thruster-limit 


I.  STROBE-CAMERA.CL 


This  code  was  written  by  Dr.  McGhee.  It  allows  the  graphically  simulation  of  the 
vehicle. 

(require  rxcw) 

(cw:Mtialize-conmon-windows) 

(defclass  strobe-camera  (rigid-body) 

((focal-length 
:accessor  focal-length 
rinitfonn  6) 

(camera-window 
raccessor  camera-window 
rinitfonn  (cwrmake-window-stream  rborders  5 

rleft  10 
rbottom  10 
rwidth  600 
rheight  600 

: title  ” strobe-camera-image” 

:activate-p  t)) 

(H-matrix 

rinitfonn  (homogeneous-transform  .3  -.3  0  -300  -90  -90)) 

(inverse-H-matrix 
raccessor  inverse-H-matrix 

rinitform  (inverse-H  (homogeneous-transform  .3  -.3  0  -300  -90  -90))) 

(enlargement-factor 
raccessor  enlargement-factor 
rinitform  30))) 

(deflnethod  move  ((camera  strobe-camera)  azimuth  elevation  roll  x  y  z) 

(setf  (H-matrix  camera)  (homogeneous-transform  azimuth  elevation  roll  x  y  z)) 

(setf  (inverse-H-matrix  camera)  (inverse-H  (H-matrix  camera)))) 

(defimethod  take-picture  ((camera  strobe-camera)  (body  rigid-body)) 

(let  ((camera-space-node-list  (mapcar  #’ (lambda  (node-location) 

(post-inultiply  (inverse-H-matrix  camera)  node-location)) 

(transfonned-node-list  body)))) 

(dolist  polygon  polygon-list  body)) 

(clip-and-draw-polygon  camera  polygon  camera-space-node-list)))) 

(definethod  chp-and-draw-polygon 
((camera  strobe-camera)  polygon  node-coord-list) 

(do*  ((initial-point  (nth  (first  polygon)  node-coord-list)) 

(firom-point  initial-point  to-point) 

(remaining-nodes  (rest  polygon)  (rest  remaining-nodes)) 

(to-point  (nth  (first  remaining-nodes)  node-coord-list) 

(if  (not  (null  (first  remaining-nodes))) 

(nth  (first  remaining-nodes)  node-coord-Iist)))) 

((uuU  to-point) 


98 


(draw-clipped-projection  camera  from-point  initial-point)) 
(draw-cl^ed-projection  camera  from-point  tofmint))) 

(definethod  draw-cIipped^)rojection  ((camera  strobe-camera)  from-point  to-point) 
(cond  ((and  (<  =  (first  from-point)  (focal-length  camera)) 

(<  =  (first  to-point)  (focal-length  camera)))  nil) 

((<  =  (first  from-point)  (focal-length  camera)) 
(draw-line-in-camera-window  camera 
^rspective-transform  camera  (from-clip  camera  from-point  to-point)) 
(perspective-transform  camera  to-point))) 

((<  =  (first  to-point)  (focal-length  camera)) 

(draw-line-in-camera-window  camera 
^rspective-transfoim  camera  from-point) 

^rspective-transform  camera  (to-clip  camera  from-point  to-point)))) 

(t  (draw-line-in-camera-window  camera 
^rspective-transform  camera  from^int) 

(perspective-transform  camera  to-point))))) 

(definethod  from-clip  ((camera  strobe-camera)  from-point  to-point) 

(let  ((scale-factor  (/  (-  (focal-length  camera)  (first  from-point)) 

(-  (first  to-point)  (first  from-point))))) 

(list  (-1-  (first  from-point) 

(*  scale-fector  (-  (first  to-point)  (first  from-point)))) 

(-)-  (second  from-point) 

(*  scale-fiictor  (-  (second  to^int)  (second  from-^oint)))) 

(-t-  (third  from-point) 

(*  scale-factor  (-  (third  to-point)  (third  from-point))))  1))) 

(definethod  to-c%  ((camera  strobe-camera)  from-point  to-point) 

(from-cl^  camera  to^wint  from-point)) 

(definethod  draw-line-in-camera-window  ((camera  strobe-camera)  start  end) 
(cw:draw-line  (camera-window  camera) 

(cw:make-position  :x  (first  start)  :y  (second  start)) 

(cw;make-position  :x  (first  end)  :y  (second  end)) 

:brush-width  0)) 

(definethod  perfective-transform  ((camera  strobe-camera)  point-in-camera-space) 
(let*  ((enlargement-fector  (enlargement-factor  camera)) 

(focal-Iengfli  (focal-length  camera)) 

(x  (first  point-in-camera-space))  ;x  axis  is  along  optical  axis 
(y  (second  point-in-camera-space))  ;y  is  out  right  side  of  camera 
(z  (third  point-in-camera-space)))  ;z  is  out  bottom  of  camera 
(Ust  (-1-  (round  (*  enlargement-fitctor  (/  (*  focal-length  y)  x))) 

300)  ;to  right  in  camera  window 

(-1-  300  (rouW  (*  enlargement-factor  (/  (*  focal-length  (-  z))  x)) 

)))))  m  camera  window 
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APPENDIX  B.  MATLAB  SOURCE  CODE 


%  Sinmlation  of  Thraster  Forces  for  AUV  in  3-D 


%  Define  global  variables 

global  m  I  g  xo  yo  20  psio  thetao  phio  omega  zeta  K  Kdot  mam  Ixx  lyy  Izz  mwam 
global  Kx  Kxdot  Ky  Kydot  Kz  Kzdot  I^si  Kpsidot  Ktheta  Ktbetadot  I^hi  Kphidot 


dl  =  1.23 
d2=1.44 
d3=0.04 
d4=1.73 
d5=1.94 


%  longitudinal  distance  between  eg  and  T1  (tbuster-1) 
%  longitudinal  distance  between  eg  and  T2 
%  vertical  distance  between  eg  and  T3p/T3s 
%  longitudinal  distance  between  eg  and  T5 
%  longitudinal  distance  between  eg  and  T6 
%  all  distances  in  feet 


%  initial  conditions 

icq=iipjt('use  default  initial  conditions?  ’,'s'); 
if  icq=  =  ’y' 

x=0;  xdot=0; 

y=0;  ydot=0; 

z=0;  zdot=0; 

psi=0;  psidot=0; 

theta =0;  thetadot=0; 

phi=0;  phidot=0; 

eke 

ic=ii5)ut('set  initial  posture  and  rates  =  ’); 


x=ic(l); 

y=ic(3); 

z=ic(5); 

psi=ic(7); 

theta=ic(9); 

phi=ic(ll); 

end 


xdot=ic(2); 

ydot=ic(4); 

zdot=ic(6); 

psidot=ic(8); 

tlietadot=ic(10); 

phidot=ic(12); 


%desired  conditions 
dc=iiq)ut(’set  desired  conditions  =  '); 
xo=dc(l);  yo=dc(2); 

psio=dc(4);  thetao=dc(5); 


zo=dc(3); 

pliio=dc(6); 


%  AUV  parameters 
Fb=389; 
g=32.2185; 
m=  12.0117; 
1=12.0117; 
lxx=2.7; 
kpdot=4.833; 
xudot=  1.0650; 


%  buouancy  force  in  lbs. 

%  gravity 

%  mass  in  slugs  (dry  weight  asssumed  to  be  387  lbs.) 
%  inertia 

lyy=42;  lzz=45; 

mqdot=  125.8594;  nrdot=9.4646; 

yvdot=  12.9541 ;  zwdot=35.2744; 
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mwam=[(Ixx+lq)dot)  (lyy+mqdot)  (Izz+nrdot)  (m+xudot)  (m+yvdot)  (m+zwdot)]; 
aI=((Iyy+mqdot)+  (Izz+nrdot))/2; 
am=((m+xudot)+  (m+yvdot)  +  (m+zwdot))/3; 
mam=[al  al  al  am  am  am]; 

%  omega  and  zeta  values 

omega=[.04  .04  .04  .04  .04  .04];  %  omega  values  for  psi,tlieta,pM,x,y,z  req>ectively 

zeta=  [.8  .8  .8  .8  .8  .8];  %  zeta  values  for  psi,theta,phi,x,y,z  respectively 


%  Gains 

K=mam.*(omega.^2); 

Kdot = 2.  *mam.  *zeta.  *omega; 


Kpsi=K(l); 

Ktheta=K(2); 

KpM=K(3); 

Kx=K(4); 

Ky=K(5); 

Kz=K(6); 


Kpsidot=Kdot(l); 

Kthetadot=Kdot(2); 

Kphidot=Kdot(3); 

Kxdot=Kdot(4); 

Kydot=Kdot(5); 

Kzdot=Kdot(6); 


%  start  and  stop  times 
t0=0; 

tfiiial=ii5>ut('settfinal  =  ’); 


%  Numerical  Integration 

vO = [x  xdot  z  zdot  theta  thetadot  psi  psidot  phi  phidot  y  ydot]’ ; 
[t,v] =ode23('vd3d’  ,t0,tfinal,v0); 


x=v(:,l);  xdot=v(:,2);  z=v(:,3);  zdot=v(:,4);  theta=v(:,5);  thetadot=v(:,6); 
psi=v(:,7);  psidot=v(:,8);  phi=v(:,9);  phidot=v(:,10);  y=v(:,ll);  ydot=v(:,12); 

%  Matrix  of  Coefficients  for  Tl,T2,T3p,T3s,T5,T6  and  its  inverse  matrix 
c=[0  0  1  0  0;0  0  0  1  1;1  1  0  0  0;dl  -d2  -d3  0  0;0  0  0  -d4  d5]; 
cinv=inv(c); 


%  Control  Equations 
Fxe=-Kx.*(x-xo)  -  Kxdot.*xdot; 

Fye=-Ky.*^-yo)  -  Kydot.*ydot; 

Fze=-Kz.*(z-zo)  -  Kzdot.*zdot  +  (Fb-m*g); 
Fe=[FxeFyeFze]'; 

%  Euler  Transformation  Matrix 
el  1 =cos(psi).*cos(theta); 

el2=(cos(psi).*sin(theta).*sin(iphi)  -  sin(psi).*cos(phi)); 
el3=(cos(psi).*sin(theta).*cos(phi)  +  sin(psi).*sin(phi)); 
e2 1 = sin(psi).  *cos(theta); 

e22=(cos(psi).*cos(phi)  +  sin(psi).*sin(theta).*sin(phi)); 
e23=(-cos^si).*sin^hi)  +  sin(psi).*sin(theta).*cos(iphi)); 
e31=-sin(theta); 
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e32= cos(tbeta).  *sin^hi) ; 
e33 = cos(tbeta).  *cos(phi); 

al=ell.*Fxe  +  e21.*Fye  +  e31.*Fze; 

a2=el2.*Fxe  +  e22.*Fye  +  e32.*Fze; 

a3=el3.*Fxe  +  e23.*Fye  +  e33.*Fze; 

a5=Fb*d3.*sin(theta)  -  Ktheta.*(theta-thetao)  -  Kthetadot.*thetadot; 

a6=-Kpsi.*(psi-psio)  -  ^sidot.*psidot: 

Thrast=cinv*[al  ’  ;a2'  ;a3 '  ;a5'  ;a6']; 

figure(l) 

subplot(321);  plot(t,x);  xlabel('time  (sec)’);  ylabel(’x  (ft)') 
subplot(323);  plot(t,y);  xlabel('time  (secy);  ylabel('y  (ft)') 
subplot(325);  plot(t,z);  xlabel('time  (sec)');  ylabel('z  (ft)’) 
sul^lot(322);  plot(t,psi);  xlabel('time  (sec)’);  ylabel('psi  (rad)’) 
sul^lot(324);  plot(t,theta);  xlabel(’time  (sec)’);  ylabel(’theta  (rad)’) 
subplot(326);  plot(t,phi);  xlabel('time  (sec)’);  ylabel(’phi  (rad)’) 

%  orient  landscape 
print  posture.anv3d 

figure(2) 

subplot(321);  plot(t,Tlirust(l,:));xlabel('tinie  (sec)’); 
ylabel(’Tl  (lbs)’);title('Aft  Vertical  Thruster-Tl’); 
siil5)lot(322);  plot(t,Tlirust(2,:));  xlabel('time  (sec)’); 
ylabel(’T2  ^bs)’);title('Fwd  Vertical  Tliruster-T2’); 
sul^lot(323);  plot(t,Thrust(4,:));  xlabel('time  (sec)’); 
ylabel(’T4  Obs)');title(’Aft  Lateral  Thruster-T4’); 
subplot(324);  plot(t,Thrust(5,:));  xlabel('tinie  (sec)’); 
yl^l(’T5  0[bs)’);title(’Fwd  Lateral  Thruster-TS’); 
sulq)lot(325);  plot(t,TIirust(3,:));  xlabel(’tiine  (sec)’); 
ylabel(’T3  ^bs)’);title(’Main  Screws  Conibined-T3’); 

%  orient  landsc^e 
print  tbrusters.auv3d 
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%  function  that  to  calculate  the  state  variables 


function  vprime  =  vd3d(t,v,par) 

%  Define  global  variables 

global  m  I  g  xo  yo  zo  psio  thetao  phio  omega  zeta  K  Kdot  mam  Ixx  lyy  Izz  mwam 
global  Kx  Kxdot  Ky  Kydot  Kz  Kzdot  Kpsi  Kpsidot  Ktheta  Kthetadot  Kphi  Kphidot 

vldot=[  v(2)]; 

v2dot=[-Kx/mwam(4)*(v(l)-xo)  -  K[xdot/mwam(4)*v(2)]; 
v3dot=[v(4)]; 

v4dot=[-Kz/mwam(6)*(v(3)-zo)  -  Kzdot/mwam(6)*v(4)]; 
v5dot=[v(6)]; 

v6dot=[-Ktheta/mwam(2)*(v(5)-thetao)  -  Kthetadot/mwam(2)*v(6)]; 
v7dot=[v(8)]; 

v8dot=[-Kpsi/mwam(3)*(v(7)-psio)  -  Kpsidot/mwam(3)*v(8)]; 
v9dot=[v(10)]; 

vl0dot=[-Kphi/mwam(l)*(v(9)-phio)  -  Kphidot/mwam(l)*v(10)]; 
vlldot=[v(12)]; 

vl2dot=[-Ky/mwam(5)*(v(ll)-yo)  -  Kydot/mwam(5)*v(12)]; 

vprime = [vldot;v2dot;v3dot;v4dot;v5dot;v6dot;v7dot;v8dot;v9dot;vl  Odot;v  1  Idot;  v  12dot] ; 
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